Merge branch 'master' of github.com:introlab/rtabmap into gtest

CMakeLists.txt

@@ -22,7 +22,7 @@ SET(CMAKE_MODULE_PATH "${PROJECT_SOURCE_DIR}/cmake_modules")
 #######################
 SET(RTABMAP_MAJOR_VERSION 0)
 SET(RTABMAP_MINOR_VERSION 23)
-SET(RTABMAP_PATCH_VERSION 6)
+SET(RTABMAP_PATCH_VERSION 7)
 SET(RTABMAP_VERSION
   ${RTABMAP_MAJOR_VERSION}.${RTABMAP_MINOR_VERSION}.${RTABMAP_PATCH_VERSION})
 
@@ -260,6 +260,7 @@ option(WITH_FASTCV        "Include FastCV support"               ON)
 option(WITH_OPENMP        "Include OpenMP support"               ON)
 option(WITH_OPENGV        "Include OpenGV support"               ON)
 option(BUILD_OPENGV       "Build OpenGV internally instead of using the system one" OFF)
+option(WITH_APRILTAG      "Include AprilTag support"             OFF)
 IF(MOBILE_BUILD)
 option(PCL_OMP            "With PCL OMP implementations"         OFF)
 ELSE()
@@ -896,6 +897,20 @@ IF(WITH_FASTCV)
     ENDIF(FastCV_FOUND)
 ENDIF(WITH_FASTCV)
 
+IF(WITH_APRILTAG)
+    FIND_PACKAGE(apriltag QUIET)
+    IF(apriltag_FOUND)
+      get_target_property(APRILTAG_LOCATION apriltag::apriltag LOCATION)
+      get_target_property(APRILTAG_INCLUDES apriltag::apriltag INTERFACE_INCLUDE_DIRECTORIES)
+      FIND_FILE(apriltag_aruco_4x4_50_header NAMES tagAruco4x4_50.h PATH_SUFFIXES aruco PATHS ${APRILTAG_INCLUDES} NO_DEFAULT_PATH)
+      SET(WITH_APRILTAG_ARUCO NO)
+      IF(apriltag_aruco_4x4_50_header)
+         SET(WITH_APRILTAG_ARUCO YES)
+      ENDIF()
+      MESSAGE(STATUS "Found apriltag (with aruco=${WITH_APRILTAG_ARUCO}): ${APRILTAG_LOCATION} ${APRILTAG_INCLUDES}")
+    ENDIF(apriltag_FOUND)
+ENDIF(WITH_APRILTAG)
+
 IF(WITH_OPENGV OR okvis_FOUND)
    if(NOT BUILD_OPENGV)
       FIND_PACKAGE(opengv QUIET)
@@ -1096,6 +1111,12 @@ ENDIF(NOT Open3D_FOUND)
 IF(NOT FastCV_FOUND)
    SET(FASTCV "//")
 ENDIF(NOT FastCV_FOUND)
+IF(NOT apriltag_FOUND)
+   SET(APRILTAG "//")
+   SET(APRILTAG_ARUCO "//")
+ELSEIF(NOT WITH_APRILTAG_ARUCO)
+   SET(APRILTAG_ARUCO "//")
+ENDIF()
 IF(NOT opengv_FOUND OR NOT WITH_OPENGV)
    SET(OPENGV "//")
 ENDIF(NOT opengv_FOUND OR NOT WITH_OPENGV)
@@ -1507,28 +1528,33 @@ ENDIF(PCL_COMPILE_OPTIONS)
 MESSAGE(STATUS "")
 MESSAGE(STATUS "Optional dependencies ('*' affects some default parameters) :")
 IF(OpenCV_FOUND)
+  IF(OPENCV_ARUCO_FOUND)
+    set(ARUCO_STR "YES")
+  ELSE()
+    set(ARUCO_STR "NO")
+  ENDIF()
   IF(OpenCV_VERSION_MAJOR EQUAL 2)
     IF(OPENCV_NONFREE_FOUND)
-      MESSAGE(STATUS " *With OpenCV 2 nonfree module (SIFT/SURF) = YES (License: Non commercial)")
+      MESSAGE(STATUS " *With OpenCV 2 nonfree module (SIFT/SURF) = YES, aruco = ${ARUCO_STR} (License: Non commercial)")
     ELSE()
-      MESSAGE(STATUS " *With OpenCV 2 nonfree module (SIFT/SURF) = NO (not found, License: BSD)")
+      MESSAGE(STATUS " *With OpenCV 2 nonfree module (SIFT/SURF) = NO, aruco = ${ARUCO_STR} (not found, License: BSD)")
     ENDIF()
   ELSE()
     IF(OPENCV_XFEATURES2D_FOUND)
       IF(NONFREE STREQUAL "//")
         IF((OpenCV_VERSION_MAJOR LESS 4) OR ((OpenCV_VERSION_MAJOR EQUAL 4) AND (OpenCV_VERSION_MINOR LESS 5)))
-          MESSAGE(STATUS " *With OpenCV ${OpenCV_VERSION} xfeatures2d = YES, nonfree = NO (License: BSD)")
+          MESSAGE(STATUS " *With OpenCV ${OpenCV_VERSION} xfeatures2d = YES, nonfree = NO, aruco = ${ARUCO_STR} (License: BSD)")
         ELSE()
-          MESSAGE(STATUS " *With OpenCV ${OpenCV_VERSION} xfeatures2d = YES, nonfree = NO (License: Apache 2)")
+          MESSAGE(STATUS " *With OpenCV ${OpenCV_VERSION} xfeatures2d = YES, nonfree = NO, aruco = ${ARUCO_STR} (License: Apache 2)")
         ENDIF()
       ELSE()
-        MESSAGE(STATUS " *With OpenCV ${OpenCV_VERSION} xfeatures2d = YES, nonfree = YES (License: Non commercial)")
+        MESSAGE(STATUS " *With OpenCV ${OpenCV_VERSION} xfeatures2d = YES, nonfree = YES, aruco = ${ARUCO_STR} (License: Non commercial)")
       ENDIF()
     ELSE()
       IF((OpenCV_VERSION_MAJOR LESS 4) OR ((OpenCV_VERSION_MAJOR EQUAL 4) AND (OpenCV_VERSION_MINOR LESS 5)))
-        MESSAGE(STATUS " *With OpenCV ${OpenCV_VERSION} xfeatures2d = NO, nonfree = NO (License: BSD)")
+        MESSAGE(STATUS " *With OpenCV ${OpenCV_VERSION} xfeatures2d = NO, nonfree = NO, aruco = ${ARUCO_STR} (License: BSD)")
       ELSE()
-        MESSAGE(STATUS " *With OpenCV ${OpenCV_VERSION} xfeatures2d = NO, nonfree = NO (License: Apache 2)")
+        MESSAGE(STATUS " *With OpenCV ${OpenCV_VERSION} xfeatures2d = NO, nonfree = NO, aruco = ${ARUCO_STR} (License: Apache 2)")
       ENDIF()
     ENDIF()
   ENDIF()
@@ -1603,6 +1629,14 @@ ELSE()
 MESSAGE(STATUS "  With FastCV               = NO (FastCV not found)")
 ENDIF()
 
+IF(apriltag_FOUND)
+MESSAGE(STATUS "  With AprilTag ${apriltag_VERSION}       = YES (aruco=${WITH_APRILTAG_ARUCO}) (License: BSD 2-Clause License)")
+ELSEIF(NOT WITH_APRILTAG)
+MESSAGE(STATUS "  With AprilTag             = NO (WITH_APRILTAG=OFF)")
+ELSE()
+MESSAGE(STATUS "  With AprilTag             = NO (apriltag not found)")
+ENDIF()
+
 IF(PDAL_FOUND)
 MESSAGE(STATUS "  With PDAL ${PDAL_VERSION}           = YES (License: BSD)")
 ELSEIF(NOT WITH_PDAL)

Version.h.in

@@ -93,6 +93,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 @TORCH@#define RTABMAP_TORCH
 @PYTHON@#define RTABMAP_PYTHON
 @MADGWICK@#define RTABMAP_MADGWICK
+@APRILTAG@#define RTABMAP_APRILTAG
+@APRILTAG_ARUCO@#define RTABMAP_APRILTAG_WITH_ARUCO
 
 #include <pcl/pcl_config.h>
 

corelib/include/rtabmap/core/MarkerDetector.h

@@ -58,6 +58,12 @@ private:
 
 class RTABMAP_CORE_EXPORT MarkerDetector {
 
+public:
+  enum Strategy {
+    kStrategyOpencv,
+    kStrategyApriltag
+  };
+    
 public:
 	MarkerDetector(const ParametersMap & parameters = ParametersMap());
 	virtual ~MarkerDetector();
@@ -84,15 +90,19 @@ public:
 		    cv::Mat * imageWithDetections = 0);
 
 private:
-#ifdef HAVE_OPENCV_ARUCO
+  Strategy strategy_;
-	cv::Ptr<cv::aruco::DetectorParameters> detectorParams_;
   float markerLength_;
+  std::map<int, float> markerLengths_;
 	float maxDepthError_;
 	float maxRange_;
 	float minRange_;
 	int dictionaryId_;
+#ifdef HAVE_OPENCV_ARUCO
+	cv::Ptr<cv::aruco::DetectorParameters> detectorParams_;
 	cv::Ptr<cv::aruco::Dictionary> dictionary_;
 #endif
+  void * apriltagLibDetector_;
+  void * apriltagLibFamily_;
 };
 
 } /* namespace rtabmap */

corelib/include/rtabmap/core/Parameters.h

@@ -933,19 +933,29 @@ class RTABMAP_CORE_EXPORT Parameters
     RTABMAP_PARAM(GridGlobal, ProbClampingMax,      float,  0.971,   "Probability clamping maximum (value between 0 and 1).");
     RTABMAP_PARAM(GridGlobal, FloodFillDepth,       unsigned int, 0, "Flood fill filter (0=disabled), used to remove empty cells outside the map. The flood fill is done at the specified depth (between 1 and 16) of the OctoMap.");
 
-    RTABMAP_PARAM(Marker, Dictionary,             int,   0,     "Dictionary to use: DICT_ARUCO_4X4_50=0, DICT_ARUCO_4X4_100=1, DICT_ARUCO_4X4_250=2, DICT_ARUCO_4X4_1000=3, DICT_ARUCO_5X5_50=4, DICT_ARUCO_5X5_100=5, DICT_ARUCO_5X5_250=6, DICT_ARUCO_5X5_1000=7, DICT_ARUCO_6X6_50=8, DICT_ARUCO_6X6_100=9, DICT_ARUCO_6X6_250=10, DICT_ARUCO_6X6_1000=11, DICT_ARUCO_7X7_50=12, DICT_ARUCO_7X7_100=13, DICT_ARUCO_7X7_250=14, DICT_ARUCO_7X7_1000=15, DICT_ARUCO_ORIGINAL = 16, DICT_APRILTAG_16h5=17, DICT_APRILTAG_25h9=18, DICT_APRILTAG_36h10=19, DICT_APRILTAG_36h11=20");
+    RTABMAP_PARAM(Marker, Strategy,               int,   0,     "Marker detection implementation: 0=OpenCV, 1=AprilTag");
-    RTABMAP_PARAM(Marker, Length,                 float, 0,     "The length (m) of the markers' side. 0 means automatic marker length estimation using the depth image (the camera should look at the marker perpendicularly for initialization).");
+    RTABMAP_PARAM(Marker, Dictionary,             int,   0,     "Dictionary to use: DICT_ARUCO_4X4_50=0, DICT_ARUCO_4X4_100=1, DICT_ARUCO_4X4_250=2, DICT_ARUCO_4X4_1000=3, DICT_ARUCO_5X5_50=4, DICT_ARUCO_5X5_100=5, DICT_ARUCO_5X5_250=6, DICT_ARUCO_5X5_1000=7, DICT_ARUCO_6X6_50=8, DICT_ARUCO_6X6_100=9, DICT_ARUCO_6X6_250=10, DICT_ARUCO_6X6_1000=11, DICT_ARUCO_7X7_50=12, DICT_ARUCO_7X7_100=13, DICT_ARUCO_7X7_250=14, DICT_ARUCO_7X7_1000=15, DICT_ARUCO_ORIGINAL = 16, DICT_APRILTAG_16h5=17, DICT_APRILTAG_25h9=18, DICT_APRILTAG_36h10=19, DICT_APRILTAG_36h11=20, DICT_ARUCO_MIP_36H12=21");
+    RTABMAP_PARAM(Marker, Length,                 float, 0,     "The length (m) of the markers' side. Value <=0 means automatic marker length estimation using the depth image (the camera should look at the marker perpendicularly for initialization). If 0, the length is estimated only on the first marker detected, then re-used for all next detections (i.e., this assumes that markers have all the same length). With <0, the length is estimated once for each unique marker, then re-used for next detections with the same marker ID.");
+    RTABMAP_PARAM_STR(Marker, Lengths,            "",           uFormat("List of markers to detect. Format is the marker's ID followed by its length (in meters), multiple markers are separated by a vertical line (\"id1 length|id2 length\"). We can also define a range of markers with \"id1:id2 length\" (id2 included). If empty, all markers of the chosen dictionary can be detected and their length is set/estimated based on %s. For example, to detect markers 12 and 14 with lengths of 8 and 15 cm respectively, and all markers between 30 and 40 with a length of 10 cm, set \"12 0.08|14 0.15|30:40 0.1\".", kMarkerLength().c_str()).c_str());
     RTABMAP_PARAM(Marker, MaxDepthError,          float, 0.01,  uFormat("Maximum depth error between all corners of a marker when estimating the marker length (when %s is 0). The smaller it is, the more perpendicular the camera should be toward the marker to initialize the length.", kMarkerLength().c_str()));
     RTABMAP_PARAM(Marker, VarianceLinear,         float, 0.001, uFormat("Linear variance to set on marker detections. If %s is enabled and %s=2 (GTSAM): it is the variance of the range factor, with 9999 to disable range factor and to do only bearing.", kMarkerVarianceOrientationIgnored().c_str(), kOptimizerStrategy().c_str()));
     RTABMAP_PARAM(Marker, VarianceAngular,        float, 0.01,  uFormat("Angular variance to set on marker detections. If %s is enabled, it is ignored with %s=1 (g2o) and it corresponds to bearing variance with %s=2 (GTSAM).", kMarkerVarianceOrientationIgnored().c_str(), kOptimizerStrategy().c_str(), kOptimizerStrategy().c_str()));
     RTABMAP_PARAM(Marker, VarianceOrientationIgnored, bool, false, uFormat("When this setting is false, the landmark's orientation is optimized during graph optimization. When this setting is true, only the position of the landmark is optimized. This can be useful when the landmark's orientation estimation is not reliable. Note that for %s=1 (g2o), only %s needs be set if we ignore orientation. For %s=2 (GTSAM), instead of optimizing the landmark's position directly, a bearing/range factor is used, with %s as the variance of the range factor (with 9999 to optimize the position with only a bearing factor) and %s as the variance of the bearing factor (pitch/yaw).", kOptimizerStrategy().c_str(), kMarkerVarianceLinear().c_str(), kOptimizerStrategy().c_str(), kMarkerVarianceLinear().c_str(), kMarkerVarianceAngular().c_str()));
-    RTABMAP_PARAM(Marker, CornerRefinementMethod, int,   0,     "Corner refinement method (0: None, 1: Subpixel, 2:contour, 3: AprilTag2). For OpenCV <3.3.0, this is \"doCornerRefinement\" parameter: set 0 for false and 1 for true.");
     RTABMAP_PARAM(Marker, MaxRange,               float, 0.0,   "Maximum range in which markers will be detected. <=0 for unlimited range.");
     RTABMAP_PARAM(Marker, MinRange,               float, 0.0,   "Miniminum range in which markers will be detected. <=0 for unlimited range.");
-    RTABMAP_PARAM_STR(Marker, Priors,             "",           "World prior locations of the markers. The map will be transformed in marker's world frame when a tag is detected. Format is the marker's ID followed by its position (angles in rad), markers are separated by vertical line (\"id1 x y z roll pitch yaw|id2 x y z roll pitch yaw\"). Example:  \"1 0 0 1 0 0 0|2 1 0 1 0 0 1.57\" (marker 2 is 1 meter forward than marker 1 with 90 deg yaw rotation).");
+    RTABMAP_PARAM_STR(Marker, Priors,             "",           "World prior locations of the markers. The map will be transformed in marker's world frame when a tag is detected. Format is the marker's ID followed by its position (angles in rad), multiple markers are separated by vertical line (\"id1 x y z roll pitch yaw|id2 x y z roll pitch yaw\"). Example:  \"1 0 0 1 0 0 0|2 1 0 1 0 0 1.57\" (marker 2 is 1 meter forward than marker 1 with 90 deg yaw rotation).");
     RTABMAP_PARAM(Marker, PriorsVarianceLinear,   float, 0.001, "Linear variance to set on marker priors.");
     RTABMAP_PARAM(Marker, PriorsVarianceAngular,  float, 0.001, "Angular variance to set on marker priors.");
 
+    RTABMAP_PARAM(MarkerAprilTag, NThreads,         int,     1,     "How many threads should be used?");
+    RTABMAP_PARAM(MarkerAprilTag, QuadDecimate,     float,   1.0,   "Detection of quads can be done on a lower-resolution image, improving speed at a cost of pose accuracy and a slight decrease in detection rate. Decoding the binary payload is still done at full resolution.");
+    RTABMAP_PARAM(MarkerAprilTag, QuadSigma,        float,   0.0,   "What Gaussian blur should be applied to the segmented image (used for quad detection?)  Parameter is the standard deviation in pixels.  Very noisy images benefit from non-zero values (e.g. 0.8).");
+    RTABMAP_PARAM(MarkerAprilTag, RefineEdges,      bool,    true,   uFormat("When true, the edges of the each quad are adjusted to \"snap to\" strong gradients nearby. This is useful when decimation is employed, as it can increase the quality of the initial quad estimate substantially. Generally recommended to be on (true). Very computationally inexpensive. Option is ignored if %s = 1.", kMarkerAprilTagQuadDecimate().c_str()));
+    RTABMAP_PARAM(MarkerAprilTag, DecodeSharpening, double,  0.25,  "How much sharpening should be done to decoded images? This can help decode small tags but may or may not help in odd lighting conditions or low light conditions.");
+    RTABMAP_PARAM(MarkerAprilTag, Debug,            bool,    false, uFormat("When true, write a variety of debugging images to the working directory where the app started (not %s) at various stages through the detection process. (Somewhat slow).", kRtabmapWorkingDirectory().c_str()));
+
+    RTABMAP_PARAM(MarkerOpenCV, CornerRefinementMethod, int,   0,     "Corner refinement method for OpenCV strategy (0: None, 1: Subpixel, 2:contour, 3: AprilTag2). For OpenCV <3.3.0, this is \"doCornerRefinement\" parameter: set 0 for false and 1 for true.");
+
     RTABMAP_PARAM(ImuFilter, MadgwickGain,                  double, 0.1,  "Gain of the filter. Higher values lead to faster convergence but more noise. Lower values lead to slower convergence but smoother signal, belongs in [0, 1].");
     RTABMAP_PARAM(ImuFilter, MadgwickZeta,                  double, 0.0,  "Gyro drift gain (approx. rad/s), belongs in [-1, 1].");
 

corelib/include/rtabmap/core/optimizer/OptimizerG2O.h

@@ -41,6 +41,12 @@ public:
 	static bool isCSparseAvailable();
 	static bool isCholmodAvailable();
 
+public:
+	static bool loadGraph(
+			const std::string & fileName,
+			std::map<int, Transform> & poses,
+			std::multimap<int, Link> & edgeConstraints);
+
 public:
 	OptimizerG2O(const ParametersMap & parameters = ParametersMap());
 	virtual ~OptimizerG2O() {}

corelib/include/rtabmap/core/optimizer/OptimizerGTSAM.h

@@ -77,6 +77,7 @@ private:
 	std::vector<ConstraintToFactor> lastAddedConstraints_;
 	int lastSwitchId_;
 	std::set<int> addedPoses_;
+	std::map<int, bool> isLandmarkWithRotation_; // persists across iSAM2 incremental calls
 	std::pair<int, std::uint64_t> lastRootFactorIndex_;
 };
 

corelib/src/CMakeLists.txt

@@ -533,6 +533,13 @@ IF(FastCV_FOUND)
 	)
 ENDIF(FastCV_FOUND)
 
+IF(apriltag_FOUND)
+	SET(LIBRARIES
+		apriltag::apriltag
+		${LIBRARIES}
+	)
+ENDIF(apriltag_FOUND)
+
 IF(opengv_FOUND)
     SET(LIBRARIES
         ${LIBRARIES}

corelib/src/Graph.cpp

@@ -218,7 +218,14 @@ bool importPoses(
 	else if(format == 4) // g2o
 	{
 		std::multimap<int, Link> constraintsTmp;
-		UERROR("Cannot import from g2o format because it is not yet supported!");
+		if(OptimizerG2O::loadGraph(filePath, poses, constraintsTmp))
+		{
+			if(constraints)
+			{
+				*constraints = constraintsTmp;
+			}
+			return true;
+		}
 		return false;
 	}
 	else

corelib/src/MarkerDetector.cpp

@@ -28,17 +28,179 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/core/MarkerDetector.h>
 #include <rtabmap/core/util2d.h>
 #include <rtabmap/utilite/ULogger.h>
+#include <rtabmap/utilite/UStl.h>
+
+#ifdef HAVE_OPENCV_ARUCO
+#if CV_MAJOR_VERSION < 4 || (CV_MAJOR_VERSION == 4 && CV_MINOR_VERSION <8)
+namespace cv{
+	namespace aruco {
+		static const int DICT_ARUCO_MIP_36h12 = 21;
+#if CV_MAJOR_VERSION < 4 || (CV_MAJOR_VERSION == 4 && CV_MINOR_VERSION <7)
+		typedef PREDEFINED_DICTIONARY_NAME PredefinedDictionaryType;
+#endif
+	}
+}
+#endif
+#endif
+
+#ifdef RTABMAP_APRILTAG
+extern "C" {
+#include "apriltag/apriltag.h"
+#include "apriltag/apriltag_pose.h"
+#ifdef RTABMAP_APRILTAG_WITH_ARUCO
+#include "apriltag/aruco/tagAruco4x4_50.h"
+#include "apriltag/aruco/tagAruco4x4_100.h"
+#include "apriltag/aruco/tagAruco4x4_250.h"
+#include "apriltag/aruco/tagAruco4x4_1000.h"
+#include "apriltag/aruco/tagAruco5x5_50.h"
+#include "apriltag/aruco/tagAruco5x5_100.h"
+#include "apriltag/aruco/tagAruco5x5_250.h"
+#include "apriltag/aruco/tagAruco5x5_1000.h"
+#include "apriltag/aruco/tagAruco6x6_50.h"
+#include "apriltag/aruco/tagAruco6x6_100.h"
+#include "apriltag/aruco/tagAruco6x6_250.h"
+#include "apriltag/aruco/tagAruco6x6_1000.h"
+#include "apriltag/aruco/tagAruco7x7_50.h"
+#include "apriltag/aruco/tagAruco7x7_100.h"
+#include "apriltag/aruco/tagAruco7x7_250.h"
+#include "apriltag/aruco/tagAruco7x7_1000.h"
+#include "apriltag/aruco/tagArucoMIP36h12.h"
+#endif
+#include "apriltag/tag16h5.h"
+#include "apriltag/tag25h9.h"
+#include "apriltag/tag36h10.h"
+#include "apriltag/tag36h11.h"
+}
+
+#ifndef HAVE_OPENCV_ARUCO
+// To match opencv::aruco module, add opencv::aruco dictionary enum
+namespace cv{
+namespace aruco {
+enum PredefinedDictionaryType {
+	DICT_4X4_50 = 0,
+	DICT_4X4_100,
+	DICT_4X4_250,
+	DICT_4X4_1000,
+	DICT_5X5_50,
+	DICT_5X5_100,
+	DICT_5X5_250,
+	DICT_5X5_1000,
+	DICT_6X6_50,
+	DICT_6X6_100,
+	DICT_6X6_250,
+	DICT_6X6_1000,
+	DICT_7X7_50,
+	DICT_7X7_100,
+	DICT_7X7_250,
+	DICT_7X7_1000,
+	DICT_ARUCO_ORIGINAL,
+	DICT_APRILTAG_16h5,
+	DICT_APRILTAG_25h9,
+	DICT_APRILTAG_36h10,
+	DICT_APRILTAG_36h11,
+	DICT_ARUCO_MIP_36h12
+};
+}
+}
+#endif
+#endif
 
 namespace rtabmap {
 
-MarkerDetector::MarkerDetector(const ParametersMap & parameters)
+#ifdef RTABMAP_APRILTAG
+
+#define ARUCO_CASE(N, TOTAL) \
+case cv::aruco::DICT_##N##X##N##_##TOTAL: \
+	af = tagAruco##N##x##N##_##TOTAL##_create(); \
+	break;
+#define APRILTAG_CASE(N1, N2) \
+case cv::aruco::DICT_APRILTAG_##N1##h##N2: \
+	af = tag##N1##h##N2##_create(); \
+	break;
+
+apriltag_family_t * createAprilTagPredefinedDictionary(int opencvArucoDictionary)
+{
+	apriltag_family_t * af = NULL;
+	switch(opencvArucoDictionary)
+	{
+		APRILTAG_CASE(16, 5)
+		APRILTAG_CASE(25, 9)
+		APRILTAG_CASE(36, 10)
+		APRILTAG_CASE(36, 11)
+#ifdef RTABMAP_APRILTAG_WITH_ARUCO
+		ARUCO_CASE(4, 50)
+		ARUCO_CASE(4, 100)
+		ARUCO_CASE(4, 250)
+		ARUCO_CASE(4, 1000)
+		ARUCO_CASE(5, 50)
+		ARUCO_CASE(5, 100)
+		ARUCO_CASE(5, 250)
+		ARUCO_CASE(5, 1000)
+		ARUCO_CASE(6, 50)
+		ARUCO_CASE(6, 100)
+		ARUCO_CASE(6, 250)
+		ARUCO_CASE(6, 1000)
+		ARUCO_CASE(7, 50)
+		ARUCO_CASE(7, 100)
+		ARUCO_CASE(7, 250)
+		ARUCO_CASE(7, 1000)
+		case cv::aruco::DICT_ARUCO_MIP_36h12:
+			af = tagArucoMIP36h12_create();
+			break;
+#endif
+		default:
+			break;
+	}
+	return af;
+}
+void destroyAprilTagDictionary(apriltag_family_t * dictionary)
+{
+	if(dictionary == NULL)
+	{
+		return;
+	}
+	const char* name = dictionary->name;
+	if(strcmp(name, "tag16h5") == 0)               tag16h5_destroy(dictionary);
+	else if(strcmp(name, "tag25h9") == 0)          tag25h9_destroy(dictionary);
+	else if(strcmp(name, "tag36h10") == 0)         tag36h10_destroy(dictionary);
+	else if(strcmp(name, "tag36h11") == 0)         tag36h11_destroy(dictionary);
+#ifdef RTABMAP_APRILTAG_WITH_ARUCO
+	else if(strcmp(name, "tagAruco4x4_50") == 0)   tagAruco4x4_50_destroy(dictionary);
+	else if(strcmp(name, "tagAruco4x4_100") == 0)  tagAruco4x4_100_destroy(dictionary);
+	else if(strcmp(name, "tagAruco4x4_250") == 0)  tagAruco4x4_250_destroy(dictionary);
+	else if(strcmp(name, "tagAruco4x4_1000") == 0) tagAruco4x4_1000_destroy(dictionary);
+	else if(strcmp(name, "tagAruco5x5_50") == 0)   tagAruco5x5_50_destroy(dictionary);
+	else if(strcmp(name, "tagAruco5x5_100") == 0)  tagAruco5x5_100_destroy(dictionary);
+	else if(strcmp(name, "tagAruco5x5_250") == 0)  tagAruco5x5_250_destroy(dictionary);
+	else if(strcmp(name, "tagAruco5x5_1000") == 0) tagAruco5x5_1000_destroy(dictionary);
+	else if(strcmp(name, "tagAruco6x6_50") == 0)   tagAruco6x6_50_destroy(dictionary);
+	else if(strcmp(name, "tagAruco6x6_100") == 0)  tagAruco6x6_100_destroy(dictionary);
+	else if(strcmp(name, "tagAruco6x6_250") == 0)  tagAruco6x6_250_destroy(dictionary);
+	else if(strcmp(name, "tagAruco6x6_1000") == 0) tagAruco6x6_1000_destroy(dictionary);
+	else if(strcmp(name, "tagAruco7x7_50") == 0)   tagAruco7x7_50_destroy(dictionary);
+	else if(strcmp(name, "tagAruco7x7_100") == 0)  tagAruco7x7_100_destroy(dictionary);
+	else if(strcmp(name, "tagAruco7x7_250") == 0)  tagAruco7x7_250_destroy(dictionary);
+	else if(strcmp(name, "tagAruco7x7_1000") == 0) tagAruco7x7_1000_destroy(dictionary);
+	else if(strcmp(name, "tagArucoMIP_36h12") == 0) tagArucoMIP36h12_destroy(dictionary);
+#endif	
+	else
+	{
+		UFATAL("AprilTag: Didn't find the right destructor for dictionary \"%s\"", name);
+	}
+}
+#endif
+
+MarkerDetector::MarkerDetector(const ParametersMap & parameters) :
+	strategy_((Strategy)Parameters::defaultMarkerStrategy()),
+	markerLength_(Parameters::defaultMarkerLength()),
+	maxDepthError_(Parameters::defaultMarkerMaxDepthError()),
+	maxRange_(Parameters::defaultMarkerMaxRange()),
+	minRange_(Parameters::defaultMarkerMinRange()),
+	dictionaryId_(Parameters::defaultMarkerDictionary()),
+	apriltagLibDetector_(NULL),
+	apriltagLibFamily_(NULL)
 {
 #ifdef HAVE_OPENCV_ARUCO
-	markerLength_ = Parameters::defaultMarkerLength();
-	maxDepthError_ = Parameters::defaultMarkerMaxDepthError();
-	maxRange_ = Parameters::defaultMarkerMaxRange();
-	minRange_ = Parameters::defaultMarkerMinRange();
-	dictionaryId_ = Parameters::defaultMarkerDictionary();
 #if CV_MAJOR_VERSION > 4 || (CV_MAJOR_VERSION == 4 && CV_MINOR_VERSION >= 7)
     detectorParams_.reset(new cv::aruco::DetectorParameters());
 #elif CV_MAJOR_VERSION > 3 || (CV_MAJOR_VERSION == 3 && CV_MINOR_VERSION >=2)
@@ -47,22 +209,107 @@ MarkerDetector::MarkerDetector(const ParametersMap & parameters)
 	detectorParams_.reset(new cv::aruco::DetectorParameters());
 #endif
 #if CV_MAJOR_VERSION > 4 || (CV_MAJOR_VERSION == 4 && CV_MINOR_VERSION >= 7)
-    detectorParams_->cornerRefinementMethod = (cv::aruco::CornerRefineMethod) Parameters::defaultMarkerCornerRefinementMethod();
+    detectorParams_->cornerRefinementMethod = (cv::aruco::CornerRefineMethod) Parameters::defaultMarkerOpenCVCornerRefinementMethod();
 #elif CV_MAJOR_VERSION > 3 || (CV_MAJOR_VERSION == 3 && CV_MINOR_VERSION >=3)
-	detectorParams_->cornerRefinementMethod = Parameters::defaultMarkerCornerRefinementMethod();
+	detectorParams_->cornerRefinementMethod = Parameters::defaultMarkerOpenCVCornerRefinementMethod();
 #else
-	detectorParams_->doCornerRefinement = Parameters::defaultMarkerCornerRefinementMethod()!=0;
+	detectorParams_->doCornerRefinement = Parameters::defaultMarkerOpenCVCornerRefinementMethod()!=0;
 #endif
+#endif
+
 	parseParameters(parameters);
-#endif
 }
 
 MarkerDetector::~MarkerDetector() {
-
+#ifdef RTABMAP_APRILTAG
+	if(apriltagLibDetector_)
+	{
+		apriltag_detector_destroy(((apriltag_detector_t*)apriltagLibDetector_));
+	}
+	if(apriltagLibFamily_)
+	{
+		destroyAprilTagDictionary((apriltag_family_t*)apriltagLibFamily_);
+	}
+#endif
 }
 
 void MarkerDetector::parseParameters(const ParametersMap & parameters)
 {
+	int strategy = strategy_;
+	Parameters::parse(parameters, Parameters::kMarkerStrategy(), strategy);
+	strategy_ = (Strategy)strategy;
+	Parameters::parse(parameters, Parameters::kMarkerLength(), markerLength_);
+	Parameters::parse(parameters, Parameters::kMarkerMaxDepthError(), maxDepthError_);
+	Parameters::parse(parameters, Parameters::kMarkerMaxRange(), maxRange_);
+	Parameters::parse(parameters, Parameters::kMarkerMinRange(), minRange_);
+	Parameters::parse(parameters, Parameters::kMarkerDictionary(), dictionaryId_);
+	
+	if(parameters.find(Parameters::kMarkerLengths()) != parameters.end())
+	{
+		markerLengths_.clear();
+		std::string strLengths;
+		Parameters::parse(parameters, Parameters::kMarkerLengths(), strLengths);
+		std::list<std::string> strList = uSplit(strLengths, '|');
+		for(std::list<std::string>::iterator iter=strList.begin(); iter!=strList.end(); ++iter)
+		{
+			std::list<std::string> items = uSplit(*iter, ' ');
+			if(items.size() != 2)
+			{
+				UERROR("Invalid string format \"%s\" for parameter %s, make "
+					"sure the values are separated by single space and/or '|'. See "
+					"description of the parameter for example. That parameter "
+					"will be ignored.",
+					strLengths.c_str(),
+					Parameters::kMarkerLengths().c_str());
+				markerLengths_.clear();
+				break;
+			}
+			else
+			{
+				float length = uStr2Float(items.back());
+				if(uStrContains(items.front(), ":"))
+				{
+					std::list<std::string> range = uSplit(items.front(), ':');
+					if(range.size() != 2)
+					{
+						UERROR("Invalid string format \"%s\" for parameter %s, make "
+							"sure the values are separated by single space and/or '|'. See "
+							"description of the parameter for example. That parameter "
+							"will be ignored.",
+							strLengths.c_str(),
+							Parameters::kMarkerLengths().c_str());
+						markerLengths_.clear();
+						break;
+					}
+					int id1 = uStr2Int(range.front());
+					int id2 = uStr2Int(range.back());
+					UDEBUG("Adding a range of markers %d -> %d with length %f", id1, id2, length);
+					for(int id=id1; id<=id2; ++id)
+					{
+						auto inserted = markerLengths_.insert(std::make_pair(id, length));
+						if(!inserted.second)
+						{
+							UWARN("Marker %d is already configured with length %f, not overwriting", inserted.first->first, inserted.first->second);
+						}
+					}
+				}
+				else {
+					int id = uStr2Int(items.front());
+					auto inserted = markerLengths_.insert(std::make_pair(id, length));
+					if(!inserted.second)
+					{
+						UWARN("Marker %d is already configured with length %f, overwriting with %f", inserted.first->first, inserted.first->second, length);
+						inserted.first->second = length;
+					}
+					else
+					{
+						UDEBUG("Added marker %d with length %f", id, length);
+					}
+				}
+			}
+		}
+	}
+
 #ifdef HAVE_OPENCV_ARUCO
 	detectorParams_->adaptiveThreshWinSizeMin = 3;
 	detectorParams_->adaptiveThreshWinSizeMax = 23;
@@ -76,13 +323,13 @@ void MarkerDetector::parseParameters(const ParametersMap & parameters)
 	detectorParams_->minMarkerDistanceRate = 0.05;
 #if CV_MAJOR_VERSION > 4 || (CV_MAJOR_VERSION == 4 && CV_MINOR_VERSION >= 7)
     int cornerRefinementMethod;
-    Parameters::parse(parameters, Parameters::kMarkerCornerRefinementMethod(), cornerRefinementMethod);
+    Parameters::parse(parameters, Parameters::kMarkerOpenCVCornerRefinementMethod(), cornerRefinementMethod);
     detectorParams_->cornerRefinementMethod = (cv::aruco::CornerRefineMethod)cornerRefinementMethod;
 #elif CV_MAJOR_VERSION > 3 || (CV_MAJOR_VERSION == 3 && CV_MINOR_VERSION >=3)
-	Parameters::parse(parameters, Parameters::kMarkerCornerRefinementMethod(), detectorParams_->cornerRefinementMethod);
+	Parameters::parse(parameters, Parameters::kMarkerOpenCVCornerRefinementMethod(), detectorParams_->cornerRefinementMethod);
 #else
 	int doCornerRefinement = detectorParams_->doCornerRefinement?1:0;
-	Parameters::parse(parameters, Parameters::kMarkerCornerRefinementMethod(), doCornerRefinement);
+	Parameters::parse(parameters, Parameters::kMarkerOpenCVCornerRefinementMethod(), doCornerRefinement);
 	detectorParams_->doCornerRefinement = doCornerRefinement!=0;
 #endif
 	detectorParams_->cornerRefinementWinSize = 5;
@@ -95,13 +342,9 @@ void MarkerDetector::parseParameters(const ParametersMap & parameters)
 	detectorParams_->minOtsuStdDev = 5.0;
 	detectorParams_->errorCorrectionRate = 0.6;
 
-	Parameters::parse(parameters, Parameters::kMarkerLength(), markerLength_);
+	
-	Parameters::parse(parameters, Parameters::kMarkerMaxDepthError(), maxDepthError_);
-	Parameters::parse(parameters, Parameters::kMarkerMaxRange(), maxRange_);
-	Parameters::parse(parameters, Parameters::kMarkerMinRange(), minRange_);
-	Parameters::parse(parameters, Parameters::kMarkerDictionary(), dictionaryId_);
 #if CV_MAJOR_VERSION < 3 || (CV_MAJOR_VERSION == 3 && (CV_MINOR_VERSION <4 || (CV_MINOR_VERSION ==4 && CV_SUBMINOR_VERSION<2)))
-	if(dictionaryId_ >= 17)
+	if(dictionaryId_ = 17 && strategy_ == 0)
 	{
 		UERROR("Cannot set AprilTag dictionary. OpenCV version should be at least 3.4.2, "
 				"current version is %s. Setting %s to default (%d)",
@@ -110,21 +353,108 @@ void MarkerDetector::parseParameters(const ParametersMap & parameters)
 				Parameters::defaultMarkerDictionary());
 		dictionaryId_ = Parameters::defaultMarkerDictionary();
 	}
+#elif CV_MAJOR_VERSION < 4 || (CV_MAJOR_VERSION == 4 && CV_MINOR_VERSION <8)
+	if(dictionaryId_ == 21 && strategy_ == 0)
+	{
+		UERROR("Cannot set ARUCO_MIP_36h12 dictionary. OpenCV version should be at least 4.8.0, "
+				"current version is %s. Setting %s to default (%d)",
+				CV_VERSION,
+				Parameters::kMarkerDictionary().c_str(),
+				Parameters::defaultMarkerDictionary());
+		dictionaryId_ = Parameters::defaultMarkerDictionary();
+	}
 #endif
 #if CV_MAJOR_VERSION > 4 || (CV_MAJOR_VERSION == 4 && CV_MINOR_VERSION >= 7)
     dictionary_.reset(new cv::aruco::Dictionary());
     *dictionary_ = cv::aruco::getPredefinedDictionary(cv::aruco::PredefinedDictionaryType(dictionaryId_));
 #elif CV_MAJOR_VERSION > 3 || (CV_MAJOR_VERSION == 3 && CV_MINOR_VERSION >=2)
-	dictionary_ = cv::aruco::getPredefinedDictionary(cv::aruco::PREDEFINED_DICTIONARY_NAME(dictionaryId_));
+	dictionary_ = cv::aruco::getPredefinedDictionary(cv::aruco::PredefinedDictionaryType(dictionaryId_));
 #else
 	dictionary_.reset(new cv::aruco::Dictionary());
-	*dictionary_ = cv::aruco::getPredefinedDictionary(cv::aruco::PREDEFINED_DICTIONARY_NAME(dictionaryId_));
+	*dictionary_ = cv::aruco::getPredefinedDictionary(cv::aruco::PredefinedDictionaryType(dictionaryId_));
 #endif
+#else
+	if(strategy_ == 0)
+	{
+#ifdef RTABMAP_APRILTAG
+		UERROR("RTAB-Map is not built with opencv-aruco library! Fallback to AprilTag strategy (%s=1).", Parameters::kMarkerStrategy().c_str());
+		strategy_ = kStrategyApriltag;
+#else
+		UERROR("RTAB-Map is not built with opencv-aruco library!");
+#endif
+	}
+#endif
+
+#ifdef RTABMAP_APRILTAG
+	if(apriltagLibDetector_ && apriltagLibFamily_) {
+		apriltag_detector_remove_family(((apriltag_detector_t*)apriltagLibDetector_), (apriltag_family_t*)apriltagLibFamily_);
+	}
+	if(apriltagLibFamily_)
+	{
+		destroyAprilTagDictionary((apriltag_family_t*)apriltagLibFamily_);
+		apriltagLibFamily_ = NULL;
+	}
+	if(strategy_ == 1)
+	{
+		if(apriltagLibDetector_ == NULL)
+		{
+			apriltagLibDetector_ = apriltag_detector_create();
+		}
+		Parameters::parse(parameters, Parameters::kMarkerAprilTagNThreads(), ((apriltag_detector_t*)apriltagLibDetector_)->nthreads);
+		Parameters::parse(parameters, Parameters::kMarkerAprilTagQuadDecimate(), ((apriltag_detector_t*)apriltagLibDetector_)->quad_decimate);
+		Parameters::parse(parameters, Parameters::kMarkerAprilTagQuadSigma(), ((apriltag_detector_t*)apriltagLibDetector_)->quad_sigma);
+		Parameters::parse(parameters, Parameters::kMarkerAprilTagRefineEdges(), ((apriltag_detector_t*)apriltagLibDetector_)->refine_edges);
+		Parameters::parse(parameters, Parameters::kMarkerAprilTagDecodeSharpening(), ((apriltag_detector_t*)apriltagLibDetector_)->decode_sharpening);
+		Parameters::parse(parameters, Parameters::kMarkerAprilTagDebug(), ((apriltag_detector_t*)apriltagLibDetector_)->debug);
+
+		cv::aruco::PredefinedDictionaryType dictFamily = cv::aruco::PredefinedDictionaryType(dictionaryId_);
+#ifndef RTABMAP_APRILTAG_WITH_ARUCO
+		if((dictFamily >= cv::aruco::DICT_4X4_50 && dictFamily < cv::aruco::DICT_APRILTAG_16h5) || dictFamily==cv::aruco::DICT_ARUCO_MIP_36h12)
+		{
+			UERROR("Cannot set aruco dictionaries with current AprilTag library version. "
+					"Use opencv-aruco implementation instead or rebuild/install latest "
+					"AprilTag from source. Setting %s to default apriltag dictionary (%d)",
+					Parameters::kMarkerDictionary().c_str(),
+					cv::aruco::DICT_APRILTAG_36h11);
+			dictionaryId_ = cv::aruco::DICT_APRILTAG_36h11;
+			dictFamily = cv::aruco::PredefinedDictionaryType(dictionaryId_);
+		}
+#endif
+		if(dictFamily == cv::aruco::DICT_ARUCO_ORIGINAL)
+		{
+			UERROR("Cannot set ARUCO_ORIGINAL (%d) dictionary with AprilTag library implementation. "
+					"Use opencv-aruco implementation instead. Setting %s to default apriltag dictionary (%d)",
+					dictionaryId_,
+					Parameters::kMarkerDictionary().c_str(),
+					cv::aruco::DICT_APRILTAG_36h11);
+			dictionaryId_ = cv::aruco::DICT_APRILTAG_36h11;
+			dictFamily = cv::aruco::PredefinedDictionaryType(dictionaryId_);
+		}
+		apriltagLibFamily_  = createAprilTagPredefinedDictionary(dictFamily);
+		UASSERT_MSG(apriltagLibFamily_ != NULL, uFormat("AprilTag library cannot be used with dictionary type %d", (int)dictionaryId_).c_str());
+		errno = 0;
+		apriltag_detector_add_family(((apriltag_detector_t*)apriltagLibDetector_), (apriltag_family_t*)apriltagLibFamily_);
+		if (errno == ENOMEM) {
+			UFATAL("Unable to add family to detector due to insufficient memory to allocate the tag-family decoder with the default maximum hamming value of 2. Try choosing an alternative tag family.");
+		}
+	}
+#else
+	if(strategy_ == 1)
+	{
+#ifdef HAVE_OPENCV_ARUCO
+		UERROR("RTAB-Map is not built with apriltag library! Fallback to OpenCV (%s=0).", Parameters::kMarkerStrategy().c_str());
+		strategy_ = kStrategyOpencv;
+#else
+		UERROR("RTAB-Map is not built with apriltag library!");
+#endif
+	}
 #endif
 }
 
+// deprecated
 std::map<int, Transform> MarkerDetector::detect(const cv::Mat & image, const CameraModel & model, const cv::Mat & depth, float * markerLengthOut, cv::Mat * imageWithDetections)
 {
+	UDEBUG("");
     std::map<int, Transform> detections;
     std::map<int, MarkerInfo> infos = detect(image, model, depth, std::map<int, float>(), imageWithDetections);
     
@@ -144,107 +474,258 @@ std::map<int, Transform> MarkerDetector::detect(const cv::Mat & image, const Cam
 std::map<int, MarkerInfo> MarkerDetector::detect(const cv::Mat & image,
                            const std::vector<CameraModel> & models,
                            const cv::Mat & depth,
-                           const std::map<int, float> & markerLengths,
+                           const std::map<int, float> & extraMarkerLengths,
                            cv::Mat * imageWithDetections)
 {
 	UASSERT(!models.empty() && !image.empty());
 	UASSERT(int((image.cols/models.size())*models.size()) == image.cols);
 	UASSERT(int((depth.cols/models.size())*models.size()) == depth.cols);
 	int subRGBWidth = image.cols/models.size();
-	int subDepthWidth = depth.cols/models.size();
 
-	std::map<int, MarkerInfo> allInfo;
-	for(size_t i=0; i<models.size(); ++i)
-	{
-		cv::Mat subImage(image, cv::Rect(subRGBWidth*i, 0, subRGBWidth, image.rows));
-		cv::Mat subDepth;
-		if(!depth.empty())
-			subDepth = cv::Mat(depth, cv::Rect(subDepthWidth*i, 0, subDepthWidth, depth.rows));
-		CameraModel model = models[i];
-		cv::Mat subImageWithDetections;
-		std::map<int, MarkerInfo> subInfo = detect(subImage, model, subDepth, markerLengths, imageWithDetections?&subImageWithDetections:0);
-		if(ULogger::level() >= ULogger::kWarning)
-		{
-			for(std::map<int, MarkerInfo>::iterator iter=subInfo.begin(); iter!=subInfo.end(); ++iter)
-			{
-				std::pair<std::map<int, MarkerInfo>::iterator, bool> inserted = allInfo.insert(*iter);
-				if(!inserted.second)
-				{
-					UWARN("Marker %d already added by another camera, ignoring detection from camera %d", iter->first, i);
-				}
-			}
-		}
-		else
-		{
-			allInfo.insert(subInfo.begin(), subInfo.end());
-		}
-		if(imageWithDetections)
-		{
-			if(i==0)
-			{
-				*imageWithDetections = cv::Mat(image.size(), subImageWithDetections.type());
-			}
-			if(!subImageWithDetections.empty())
-			{
-				subImageWithDetections.copyTo(cv::Mat(*imageWithDetections, cv::Rect(subRGBWidth*i, 0, subRGBWidth, image.rows)));
-			}
-		}
-	}
-	return allInfo;
-}
-
-std::map<int, MarkerInfo> MarkerDetector::detect(const cv::Mat & image,
-                           const CameraModel & model,
-                           const cv::Mat & depth,
-                           const std::map<int, float> & markerLengths,
-                           cv::Mat * imageWithDetections)
-{
-	if(!image.empty() && image.cols != model.imageWidth())
-	{
-		UERROR("This method cannot handle multi-camera marker detection, use the other function version supporting it.");
-		return std::map<int, MarkerInfo>();
-	}
-
-    std::map<int, MarkerInfo> detections;
-
-#ifdef HAVE_OPENCV_ARUCO
-
-	std::vector< int > ids;
-	std::vector< std::vector< cv::Point2f > > corners, rejected;
-	std::vector< cv::Vec3d > rvecs, tvecs;
-
-	// detect markers and estimate pose
-#if CV_MAJOR_VERSION > 3 || (CV_MAJOR_VERSION == 3 && CV_MINOR_VERSION >=2)
-	cv::aruco::detectMarkers(image, dictionary_, corners, ids, detectorParams_, rejected);
-#else
-	cv::aruco::detectMarkers(image, *dictionary_, corners, ids, *detectorParams_, rejected);
-#endif
-	UDEBUG("Markers detected=%d rejected=%d", (int)ids.size(), (int)rejected.size());
-	if(ids.size() > 0)
-	{
 	float rgbToDepthFactorX = 1.0f;
 	float rgbToDepthFactorY = 1.0f;
 	if(!depth.empty())
 	{
-			rgbToDepthFactorX = 1.0f/(model.imageWidth()>0?float(model.imageWidth())/float(depth.cols):1.0f);
+		rgbToDepthFactorX = float(depth.cols) / float(image.cols);
-			rgbToDepthFactorY = 1.0f/(model.imageHeight()>0?float(model.imageHeight())/float(depth.rows):1.0f);
+		rgbToDepthFactorY = float(depth.rows) / float(image.rows);
 	}
 	else if(markerLength_ == 0)
 	{
 		if(depth.empty())
 		{
 			UERROR("Depth image is empty, please set %s parameter to non-null.", Parameters::kMarkerLength().c_str());
-                return detections;
+			return std::map<int, MarkerInfo>();
 		}
 	}
 
-		cv::aruco::estimatePoseSingleMarkers(corners, markerLength_<=0.0?1.0f:markerLength_, model.K(), model.D(), rvecs, tvecs);
+	std::vector< int > ids;
+	std::vector< int > cams;
+	std::vector< std::vector< cv::Point2f > > corners; // in stitched image
+	std::vector<Transform> poses; // in optical frame
+
+	// detect markers and estimate pose
+	if(strategy_ == kStrategyApriltag)
+	{
+#ifdef RTABMAP_APRILTAG
+		cv::Mat grayImage = image;
+		if(image.channels() > 1)
+		{
+			cv::cvtColor(image, grayImage, cv::COLOR_BGR2GRAY);
+		}
+		image_u8_t im = {grayImage.cols, grayImage.rows, (int)grayImage.step, grayImage.data};
+
+		errno = 0;
+		zarray_t *apriltagDetections = apriltag_detector_detect(((apriltag_detector_t*)apriltagLibDetector_), &im);
+		if (errno == EAGAIN) {
+			UERROR("Unable to create the %d threads requested.", ((apriltag_detector_t*)apriltagLibDetector_)->nthreads);
+			if(apriltagDetections)
+			{
+				apriltag_detections_destroy(apriltagDetections);
+			}
+			return std::map<int, MarkerInfo>();
+		}
+
+		std::set<int> idsAdded;
+		for (int i = 0; i < zarray_size(apriltagDetections); i++)
+		{
+            apriltag_detection_t *det;
+            zarray_get(apriltagDetections, i, &det);
+
+			// Which camera?
+			int cameraIndex = int(det->c[0]) / subRGBWidth;
+			if(cameraIndex<0 || cameraIndex>=(int)models.size())
+			{
+				UWARN("Marker %d detected outside the image! camera index=%d (models=%ld subWidth=%d) marker center=(%f,%f). Ignoring...",
+					det->id, cameraIndex, models.size(), subRGBWidth, det->c[0], det->c[1]);
+				continue;
+			}
+
+			if(!markerLengths_.empty() &&
+				markerLengths_.find(det->id) == markerLengths_.end() && 
+				extraMarkerLengths.find(det->id) == extraMarkerLengths.end())
+			{
+				UDEBUG("Ignoring marker %d because it is not in the list of expected markers (see %s)",
+					det->id,
+					Parameters::kMarkerLengths().c_str());
+				continue;
+			}
+			if(idsAdded.find(det->id)!=idsAdded.end())
+			{
+				UWARN("Marker %d already added by another camera, ignoring detection from camera %d", det->id, cameraIndex);
+				continue;
+			}
+
+			const CameraModel & model = models[cameraIndex];
+			float offsetX = cameraIndex*subRGBWidth;
+
+			// Convert detection in local camera
+			det->c[0] -= offsetX;
+			for(int i=0; i<4; ++i)
+			{
+				det->p[i][0] -= offsetX;
+			}
+
+			// Shift the homography to match the local-camera pixel frame:
+			// pre-multiply H by T = [[1,0,-offsetX],[0,1,0],[0,0,1]] so it
+			// stays consistent with the shifted corners. estimate_tag_pose()
+			// seeds its iterative refinement from H, so leaving it stale
+			// produces a bad initial guess (often triggers the AprilTag
+			// "more than one new minimum found" debug print).
+			MATD_EL(det->H, 0, 0) -= offsetX * MATD_EL(det->H, 2, 0);
+			MATD_EL(det->H, 0, 1) -= offsetX * MATD_EL(det->H, 2, 1);
+			MATD_EL(det->H, 0, 2) -= offsetX * MATD_EL(det->H, 2, 2);
+
+			apriltag_detection_info_t info;
+			info.det = det;
+			info.tagsize = 1.0f;
+			info.fx = model.fx();
+			info.fy = model.fy();
+			info.cx = model.cx();
+			info.cy = model.cy();
+
+			// Then call estimate_tag_pose.
+			apriltag_pose_t pose;
+			double err = estimate_tag_pose(&info, &pose);
+
+			if (pose.R && pose.t)
+			{
+				if(MATD_EL(pose.t, 2, 0) > 0)
+				{
+					Transform t(MATD_EL(pose.R, 0, 0), MATD_EL(pose.R, 0, 1), MATD_EL(pose.R, 0, 2), MATD_EL(pose.t, 0, 0),
+								MATD_EL(pose.R, 1, 0), MATD_EL(pose.R, 1, 1), MATD_EL(pose.R, 1, 2), MATD_EL(pose.t, 1, 0),
+								MATD_EL(pose.R, 2, 0), MATD_EL(pose.R, 2, 1), MATD_EL(pose.R, 2, 2), MATD_EL(pose.t, 2, 0));
+					poses.push_back(t*Transform(-1,0,0,0, 0,1,0,0, 0,0,-1,0)); // Flip tag to match same frame than OpenCV (+x->-x, +z->-z)
+
+					corners.push_back(std::vector<cv::Point2f>(4));
+					for(int i=0; i<4; ++i)
+					{
+						// reconvert in original stitched image
+						corners.back()[i].x = det->p[i][0]+offsetX;
+						corners.back()[i].y = det->p[i][1];
+					}
+					ids.push_back(det->id);
+					cams.push_back(cameraIndex);
+					UDEBUG("Add marker %d (err = %f)", det->id, err);
+					idsAdded.insert(det->id);
+				}
+				else
+				{
+					UWARN("Skipping %d because its estimated pose is behind the camera %d", det->id, cameraIndex);
+				}
+			}
+			else
+			{
+				UWARN("Failed to compute pose for marker %d, ignoring...", det->id);
+			}
+
+			// Free pose memory
+			if (pose.R) matd_destroy(pose.R);
+			if (pose.t) matd_destroy(pose.t);
+        }
+
+		apriltag_detections_destroy(apriltagDetections);
+#else
+		UERROR("RTAB-Map is not built with apriltag library.");
+#endif
+	}
+	else // opencv
+	{
+		std::vector< int > cvIds;
+		std::vector< std::vector< cv::Point2f > > cvCorners, cvRejected;
+#ifdef HAVE_OPENCV_ARUCO
+#if CV_MAJOR_VERSION > 3 || (CV_MAJOR_VERSION == 3 && CV_MINOR_VERSION >=2)
+		cv::aruco::detectMarkers(image, dictionary_, cvCorners, cvIds, detectorParams_, cvRejected);
+#else
+		cv::aruco::detectMarkers(image, *dictionary_, cvCorners, cvIds, *detectorParams_, cvRejected);
+#endif
+		UDEBUG("Markers detected=%d rejected=%d", (int)cvIds.size(), (int)cvRejected.size());
+
+		// split detections per camera
+		std::set<int> idsAdded;
+		std::vector< std::vector< std::vector< cv::Point2f > > > cvCornersPerCam(models.size());
+		std::vector< std::vector< int > > cvIdsPerCam(models.size());
+		for(size_t i=0; i<cvIds.size(); ++i)
+		{
+			int id = cvIds[i];
+
+			// Which camera?
+			int cameraIndex = int(cvCorners[i][0].x) / subRGBWidth;
+			if(cameraIndex<0 || cameraIndex>=(int)models.size())
+			{
+				UWARN("Marker %d detected outside the image! camera index=%d (models=%ld subWidth=%d) marker center=(%f,%f). Ignoring...",
+					id, cameraIndex, models.size(), subRGBWidth, cvCorners[i][0].x, cvCorners[i][0].y);
+				continue;
+			}
+
+			if(!markerLengths_.empty() &&
+				markerLengths_.find(id) == markerLengths_.end() && 
+				extraMarkerLengths.find(id) == extraMarkerLengths.end())
+			{
+				UDEBUG("Ignoring marker %d because it is not in the list of expected markers (see %s)",
+					id,
+					Parameters::kMarkerLengths().c_str());
+				continue;
+			}
+			if(idsAdded.find(id) != idsAdded.end())
+			{
+				UWARN("Marker %d already added by another camera, ignoring detection from camera %d", id, cameraIndex);
+				continue;
+			}
+
+			float offsetX = cameraIndex*subRGBWidth;
+			for(size_t j=0; j<cvCorners[i].size(); ++j)
+			{
+				cvCorners[i][j].x -= offsetX;
+			}
+			
+			cvCornersPerCam[cameraIndex].push_back(cvCorners[i]);
+			cvIdsPerCam[cameraIndex].push_back(id);
+			idsAdded.insert(id);
+			UDEBUG("Detected marker %d on camera %d", id, cameraIndex);
+		}
+
+		for(size_t cam=0; cam < cvCornersPerCam.size(); ++cam)
+		{
+			std::vector< cv::Vec3d > rvecs, tvecs;
+			const CameraModel & model = models[cam];
+			cv::aruco::estimatePoseSingleMarkers(cvCornersPerCam[cam], 1.0f, model.K(), model.D(), rvecs, tvecs);
+			float offsetX = cam*subRGBWidth;
+			for(size_t i=0; i<cvIdsPerCam[cam].size(); ++i)
+			{
+				if(tvecs[i].val[2] <=0)
+				{
+					UWARN("Skipping %d because its estimated pose is behind the camera %d", cvIdsPerCam[cam][i], cam);
+					continue;
+				}
+				cv::Mat R;
+				cv::Rodrigues(rvecs[i], R);
+				Transform t(R.at<double>(0,0), R.at<double>(0,1), R.at<double>(0,2), tvecs[i].val[0],
+							R.at<double>(1,0), R.at<double>(1,1), R.at<double>(1,2), tvecs[i].val[1],
+							R.at<double>(2,0), R.at<double>(2,1), R.at<double>(2,2), tvecs[i].val[2]);
+				poses.push_back(t);
+				ids.push_back(cvIdsPerCam[cam][i]);
+				cams.push_back(cam);
+				// reconvert in original stitched image
+				for(size_t j=0; j<cvCornersPerCam[cam][i].size(); ++j)
+				{
+					cvCornersPerCam[cam][i][j].x += offsetX;
+				}
+				corners.push_back(cvCornersPerCam[cam][i]);
+			}
+		}	
+#else
+	UERROR("RTAB-Map is not built with \"aruco\" module from OpenCV.");
+#endif
+	}
+
+	// Estimate scale and fill up output detections
 	std::vector<float> scales;
+	std::map<int, MarkerInfo> detections;
 	for(size_t i=0; i<ids.size(); ++i)
 	{
 		float length = 0.0f;
-            std::map<int, float>::const_iterator findIter = markerLengths.find(ids[i]);
+		std::map<int, float>::const_iterator findIter = extraMarkerLengths.find(ids[i]);
-            if(!depth.empty() && (markerLength_ == 0 || (markerLength_<0 && findIter==markerLengths.end())))
+		if(markerLengths_.empty() && !depth.empty() && (markerLength_ == 0 || (markerLength_<0 && findIter==extraMarkerLengths.end())))
 		{
 			float d = util2d::getDepth(depth, (corners[i][0].x + (corners[i][2].x-corners[i][0].x)/2.0f)*rgbToDepthFactorX, (corners[i][0].y + (corners[i][2].y-corners[i][0].y)/2.0f)*rgbToDepthFactorY, true, 0.02f, true);
 			float d1 = util2d::getDepth(depth, corners[i][0].x*rgbToDepthFactorX, corners[i][0].y*rgbToDepthFactorY, true, 0.02f, true);
@@ -256,7 +737,7 @@ std::map<int, MarkerInfo> MarkerDetector::detect(const cv::Mat & image,
 			// best depth estimation)
 			if(d>0 && d1>0 && d2>0 && d3>0 && d4>0)
 			{
-                    float scale = d/tvecs[i].val[2];
+				float scale = d / poses[i].z();
 				
 				if( fabs(d-d1) < maxDepthError_ &&
 					fabs(d-d2) < maxDepthError_ &&
@@ -265,8 +746,7 @@ std::map<int, MarkerInfo> MarkerDetector::detect(const cv::Mat & image,
 				{
 					length = scale;
 					scales.push_back(length);
-						tvecs[i] *= scales.back();
+					UWARN("Automatic marker length estimation: id=%d depth=%fm length=%fm", ids[i], d, length);
-						UWARN("Automatic marker length estimation: id=%d depth=%fm length=%fm", ids[i], d, scales.back());
 				}
 				else
 				{
@@ -292,19 +772,41 @@ std::map<int, MarkerInfo> MarkerDetector::detect(const cv::Mat & image,
 				continue;
 			}
 		}
-            else if(markerLength_ < 0)
+		else if(!markerLengths_.empty() || findIter!=extraMarkerLengths.end())
 		{
-                if(findIter!=markerLengths.end())
+			std::map<int, float>::const_iterator paramIter = markerLengths_.find(ids[i]);
+			if(paramIter != markerLengths_.end())
 			{
+				if(findIter!=extraMarkerLengths.end() && findIter->second != paramIter->second)
+				{
+					UWARN("Marker's length of %d is defined both in extra lengths "
+						"(%f m) and the parameter %s (%f m), we will use the length "
+						"from extra lengths.",
+						findIter->second,
+						Parameters::kMarkerLengths().c_str(),
+						paramIter->second);
 					length = findIter->second;
-                    tvecs[i] *= length;
 				}
 				else
 				{
-                    UWARN("Cannot find marker length for marker %d, ignoring this marker (count=%d)", ids[i], (int)markerLengths.size());
+					length = paramIter->second;
+				}
+			}
+			else if(findIter!=extraMarkerLengths.end())
+			{
+				length = findIter->second;
+			}
+			else
+			{
+				UERROR("Not supposed to reach this case, ignoring detection %d", ids[i]);
 				continue;
 			}
 		}
+		else if(markerLength_ < 0)
+		{
+			UWARN("Cannot find marker length for marker %d, ignoring this marker (count=%d)", ids[i], (int)extraMarkerLengths.size());
+			continue;
+		}
 		else if(markerLength_ > 0)
 		{
 			length = markerLength_;
@@ -314,19 +816,28 @@ std::map<int, MarkerInfo> MarkerDetector::detect(const cv::Mat & image,
 			continue;
 		}
 
+		UASSERT(length > 0.0f);
+		poses[i].x() *= length;
+		poses[i].y() *= length;
+		poses[i].z() *= length;
+
 		// Limit the detection range to be between the min / max range.
 		// If the ranges are -1, allow any detection within that direction.
-			if((maxRange_ <= 0 || tvecs[i].val[2] < maxRange_) &&
+		if((maxRange_ <= 0 || poses[i].z() < maxRange_) &&
-				(minRange_ <= 0 || tvecs[i].val[2] > minRange_))
+			(minRange_ <= 0 || poses[i].z() > minRange_))
 		{
-				cv::Mat R;
+			Transform pose = models[cams[i]].localTransform() * poses[i];
-				cv::Rodrigues(rvecs[i], R);
-				Transform t(R.at<double>(0,0), R.at<double>(0,1), R.at<double>(0,2), tvecs[i].val[0],
-								 R.at<double>(1,0), R.at<double>(1,1), R.at<double>(1,2), tvecs[i].val[1],
-								 R.at<double>(2,0), R.at<double>(2,1), R.at<double>(2,2), tvecs[i].val[2]);
-				Transform pose = model.localTransform() * t;
 			detections.insert(std::make_pair(ids[i], MarkerInfo(ids[i], length, pose)));
-				UDEBUG("Marker %d detected in base_link: %s, optical_link=%s, local transform=%s", ids[i], pose.prettyPrint().c_str(), t.prettyPrint().c_str(), model.localTransform().prettyPrint().c_str());
+			UDEBUG("Marker %d detected in base_link: %s, optical_link=%s, local transform=%s",
+				ids[i],
+				pose.prettyPrint().c_str(),
+				poses[i].prettyPrint().c_str(),
+				models[cams[i]].localTransform().prettyPrint().c_str());
+		}
+		else
+		{
+			UDEBUG("Filtered marker %d by distance (min=%f max=%f value=%f) from the camera %d",
+				ids[i], minRange_, maxRange_, poses[i].z(), cams[i]);
 		}
 	}
 	if(markerLength_ == 0 && !scales.empty())
@@ -343,9 +854,12 @@ std::map<int, MarkerInfo> MarkerDetector::detect(const cv::Mat & image,
 					UWARN("The marker's length detected between 2 of the "
 							"markers doesn't match (%fm vs %fm)."
 							"Parameter %s can be set to non-null to skip automatic "
-							  "marker length estimation. Detections are ignored.",
+							"marker length estimation or set to a negative value to "
+							"estimate length for each marker. The current %ld detections "
+							"are ignored!",
 							scales[i], scales[0],
-							  Parameters::kMarkerLength().c_str());
+							Parameters::kMarkerLength().c_str(),
+							detections.size());
 					detections.clear();
 					return detections;
 				}
@@ -359,7 +873,6 @@ std::map<int, MarkerInfo> MarkerDetector::detect(const cv::Mat & image,
 		markerLength_ = sum/float(scales.size());
 		UWARN("Final marker length estimated = %fm, max error=%fm (used for subsequent detections)", markerLength_, maxError);
 	}
-	}
 
 	if(imageWithDetections)
 	{
@@ -371,31 +884,61 @@ std::map<int, MarkerInfo> MarkerDetector::detect(const cv::Mat & image,
 		{
 			image.copyTo(*imageWithDetections);
 		}
+
 		if(!ids.empty())
 		{
+#ifdef HAVE_OPENCV_ARUCO
 			cv::aruco::drawDetectedMarkers(*imageWithDetections, corners, ids);
-
+#else
+			UWARN("RTAB-Map is not built with \"aruco\" module from OpenCV. Cannot draw markers on image.");
+#endif
 			for(unsigned int i = 0; i < ids.size(); i++)
 			{
                 std::map<int, MarkerInfo>::iterator iter = detections.find(ids[i]);
                 if(iter!=detections.end())
                 {
+					int cam = cams[i];
+					UASSERT(cam >=0 && cam < (int)models.size());
+					const CameraModel & model = models[cam];
+					int roix = subRGBWidth*cam;
+					UASSERT(roix >=0 && roix <= imageWithDetections->cols - subRGBWidth);
+					cv::Mat subImage(*imageWithDetections, cv::Rect(roix, 0, subRGBWidth, imageWithDetections->rows));
+					cv::Vec3d rvec;
+					cv::Vec3d tvec(poses[i].x(), poses[i].y(), poses[i].z());
+					cv::Mat R;
+					poses[i].rotationMatrix().convertTo(R, CV_64F);
+					cv::Rodrigues(R, rvec);			
 #if CV_MAJOR_VERSION > 4 || (CV_MAJOR_VERSION == 4 && (CV_MINOR_VERSION >1 || (CV_MINOR_VERSION==1 && CV_PATCH_VERSION>=1)))
-                    cv::drawFrameAxes(*imageWithDetections, model.K(), model.D(), rvecs[i], tvecs[i], iter->second.length() * 0.5f);
+                    cv::drawFrameAxes(subImage, model.K(), model.D(), rvec, tvec, iter->second.length() * 0.5f);
-#else
+#elif defined(HAVE_OPENCV_ARUCO)
-                    cv::aruco::drawAxis(*imageWithDetections, model.K(), model.D(), rvecs[i], tvecs[i], iter->second.length() * 0.5f);
+                    cv::aruco::drawAxis(subImage, model.K(), model.D(), rvec, tvec, iter->second.length() * 0.5f);
 #endif
                 }
 			}
 		}
 	}
 
-#else
-	UERROR("RTAB-Map is not built with \"aruco\" module from OpenCV.");
-#endif
-
 	return detections;
 }
 
+std::map<int, MarkerInfo> MarkerDetector::detect(const cv::Mat & image,
+                           const CameraModel & model,
+                           const cv::Mat & depth,
+                           const std::map<int, float> & markerLengths,
+                           cv::Mat * imageWithDetections)
+{
+	UDEBUG("");
+	if(!image.empty() && image.cols != model.imageWidth())
+	{
+		UERROR("This method cannot handle multi-camera marker detection, use the other function version supporting it.");
+		return std::map<int, MarkerInfo>();
+	}
+
+    std::vector<CameraModel> models;
+	models.push_back(model);
+
+	return detect(image, models, depth, markerLengths, imageWithDetections);
+}
+
 
 } /* namespace rtabmap */

corelib/src/Parameters.cpp

@@ -238,6 +238,9 @@ const std::map<std::string, std::pair<bool, std::string> > & Parameters::getRemo
 	{
 		// removed parameters
 
+		// 0.23.7
+		removedParameters_.insert(std::make_pair("Marker/CornerRefinementMethod", std::make_pair(true, Parameters::kMarkerOpenCVCornerRefinementMethod())));
+
 		// 0.23.1
 		removedParameters_.insert(std::make_pair("OdomVINS/ConfigPath",    std::make_pair(true, Parameters::kOdomVINSFusionConfigPath())));
 
@@ -290,7 +293,7 @@ const std::map<std::string, std::pair<bool, std::string> > & Parameters::getRemo
 		removedParameters_.insert(std::make_pair("Aruco/MaxDepthError",          std::make_pair(true,  Parameters::kMarkerMaxDepthError())));
 		removedParameters_.insert(std::make_pair("Aruco/VarianceLinear",         std::make_pair(true,  Parameters::kMarkerVarianceLinear())));
 		removedParameters_.insert(std::make_pair("Aruco/VarianceAngular",        std::make_pair(true,  Parameters::kMarkerVarianceAngular())));
-		removedParameters_.insert(std::make_pair("Aruco/CornerRefinementMethod", std::make_pair(true,  Parameters::kMarkerCornerRefinementMethod())));
+		removedParameters_.insert(std::make_pair("Aruco/CornerRefinementMethod", std::make_pair(true,  Parameters::kMarkerOpenCVCornerRefinementMethod())));
 
 		// 0.17.5
 		removedParameters_.insert(std::make_pair("Grid/OctoMapOccupancyThr",     std::make_pair(true,  Parameters::kGridGlobalOccupancyThr())));
@@ -681,6 +684,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 AprilTag:";
+#ifdef RTABMAP_APRILTAG
+				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 OpenGV:";
 #ifdef RTABMAP_OPENGV

corelib/src/Rtabmap.cpp

@@ -4504,8 +4504,7 @@ bool Rtabmap::process(
 			(smallDisplacement || tooFastMovement) &&
 			_loopClosureHypothesis.first == 0 &&
 			lastProximitySpaceClosureId == 0 &&
-			!delayedLocalization &&
+			!delayedLocalization)
-			(rejectedLoopClosure || landmarksDetected.empty()))
 	{
 		_odomCachePoses.erase(signatureRemoved);
 		for(std::multimap<int, Link>::iterator iter=_odomCacheConstraints.begin(); iter!=_odomCacheConstraints.end();)

corelib/src/optimizer/OptimizerG2O.cpp

@@ -2135,6 +2135,390 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
 	return optimizedPoses;
 }
 
+bool OptimizerG2O::loadGraph(
+		const std::string & fileName,
+		std::map<int, Transform> & poses,
+		std::multimap<int, Link> & edgeConstraints)
+{
+	FILE * file = 0;
+#ifdef _MSC_VER
+	fopen_s(&file, fileName.c_str(), "r");
+#else
+	file = fopen(fileName.c_str(), "r");
+#endif
+
+	if(!file)
+	{
+		UERROR("Cannot open file %s", fileName.c_str());
+		return false;
+	}
+
+	// saveGraph() writes landmarks (originally negative ids, remapped to
+	// landmarkOffset - id) first in DESCENDING file-id order, then regular
+	// poses in ASCENDING file-id order. We recover landmarkOffset from this
+	// order to restore the original negative landmark ids.
+	struct VertexEntry {
+		int fileId;
+		Transform transform;
+		bool definitelyLandmark; // VERTEX_XY / VERTEX_TRACKXYZ
+	};
+	struct EdgeEntry {
+		int from;
+		int to;
+		Link::Type type;
+		Transform transform;
+		cv::Mat info;
+		bool isPrior;             // from==to, prior on a single vertex
+		bool hasLandmarkEndpoint; // tag implies a landmark on one side
+	};
+	std::vector<VertexEntry> verticesList;
+	std::vector<EdgeEntry> edgesList;
+
+	char line[2048];
+	while(fgets(line, 2048, file) != NULL)
+	{
+		std::list<std::string> tokenList = uSplit(uReplaceChar(uReplaceChar(line, '\n', ' '), '\r', ' '), ' ');
+		std::vector<std::string> v;
+		v.reserve(tokenList.size());
+		for(std::list<std::string>::const_iterator iter = tokenList.begin(); iter != tokenList.end(); ++iter)
+		{
+			if(!iter->empty())
+			{
+				v.push_back(*iter);
+			}
+		}
+		if(v.empty())
+		{
+			continue;
+		}
+		const std::string & tag = v[0];
+
+		// Skip parameters, switch helpers and unrelated entries
+		if(tag == "PARAMS_SE2OFFSET" || tag == "PARAMS_SE3OFFSET" ||
+		   tag == "VERTEX_SWITCH" || tag == "EDGE_SWITCH_PRIOR")
+		{
+			continue;
+		}
+
+		if(tag == "VERTEX_SE2" && v.size() == 5)
+		{
+			VertexEntry e;
+			e.fileId = atoi(v[1].c_str());
+			e.transform = Transform(uStr2Float(v[2]), uStr2Float(v[3]), uStr2Float(v[4]));
+			e.definitelyLandmark = false;
+			verticesList.push_back(e);
+		}
+		else if(tag == "VERTEX_XY" && v.size() == 4)
+		{
+			VertexEntry e;
+			e.fileId = atoi(v[1].c_str());
+			e.transform = Transform(uStr2Float(v[2]), uStr2Float(v[3]), 0);
+			e.definitelyLandmark = true;
+			verticesList.push_back(e);
+		}
+		else if(tag == "VERTEX_SE3:QUAT" && v.size() == 9)
+		{
+			VertexEntry e;
+			e.fileId = atoi(v[1].c_str());
+			e.transform = Transform(uStr2Float(v[2]), uStr2Float(v[3]), uStr2Float(v[4]),
+									uStr2Float(v[5]), uStr2Float(v[6]), uStr2Float(v[7]), uStr2Float(v[8]));
+			e.definitelyLandmark = false;
+			verticesList.push_back(e);
+		}
+		else if(tag == "VERTEX_TRACKXYZ" && v.size() == 5)
+		{
+			VertexEntry e;
+			e.fileId = atoi(v[1].c_str());
+			e.transform = Transform(uStr2Float(v[2]), uStr2Float(v[3]), uStr2Float(v[4]), 0, 0, 0);
+			e.definitelyLandmark = true;
+			verticesList.push_back(e);
+		}
+		else if(tag == "EDGE_SE2" && v.size() == 12)
+		{
+			EdgeEntry e;
+			e.from = atoi(v[1].c_str());
+			e.to   = atoi(v[2].c_str());
+			e.transform = Transform(uStr2Float(v[3]), uStr2Float(v[4]), uStr2Float(v[5]));
+			e.info = cv::Mat::eye(6, 6, CV_64FC1);
+			e.info.at<double>(0, 0) = uStr2Double(v[6]);
+			e.info.at<double>(0, 1) = e.info.at<double>(1, 0) = uStr2Double(v[7]);
+			e.info.at<double>(0, 5) = e.info.at<double>(5, 0) = uStr2Double(v[8]);
+			e.info.at<double>(1, 1) = uStr2Double(v[9]);
+			e.info.at<double>(1, 5) = e.info.at<double>(5, 1) = uStr2Double(v[10]);
+			e.info.at<double>(5, 5) = uStr2Double(v[11]);
+			e.type = Link::kUndef; // disambiguated after we know landmarkOffset
+			e.isPrior = false;
+			e.hasLandmarkEndpoint = false;
+			edgesList.push_back(e);
+		}
+		else if(tag == "EDGE_SE2_XY" && v.size() == 8)
+		{
+			EdgeEntry e;
+			e.from = atoi(v[1].c_str());
+			e.to   = atoi(v[2].c_str());
+			e.transform = Transform(uStr2Float(v[3]), uStr2Float(v[4]), 0);
+			e.info = cv::Mat::eye(6, 6, CV_64FC1);
+			e.info.at<double>(0, 0) = uStr2Double(v[5]);
+			e.info.at<double>(0, 1) = e.info.at<double>(1, 0) = uStr2Double(v[6]);
+			e.info.at<double>(1, 1) = uStr2Double(v[7]);
+			e.type = Link::kLandmark;
+			e.isPrior = false;
+			e.hasLandmarkEndpoint = true;
+			edgesList.push_back(e);
+		}
+		else if((tag == "EDGE_SE3:QUAT" || tag == "EDGE_SE3") && v.size() == 31)
+		{
+			EdgeEntry e;
+			e.from = atoi(v[1].c_str());
+			e.to   = atoi(v[2].c_str());
+			e.transform = Transform(uStr2Float(v[3]), uStr2Float(v[4]), uStr2Float(v[5]),
+									uStr2Float(v[6]), uStr2Float(v[7]), uStr2Float(v[8]), uStr2Float(v[9]));
+			e.info = cv::Mat::eye(6, 6, CV_64FC1);
+			int idx = 10;
+			for(int r = 0; r < 6; ++r)
+			{
+				for(int c = r; c < 6; ++c)
+				{
+					e.info.at<double>(r, c) = uStr2Double(v[idx++]);
+					if(r != c) e.info.at<double>(c, r) = e.info.at<double>(r, c);
+				}
+			}
+			// EDGE_SE3 (no :QUAT) is the landmark variant emitted by saveGraph
+			bool landmarkTag = (tag == "EDGE_SE3");
+			e.type = landmarkTag ? Link::kLandmark : Link::kUndef;
+			e.isPrior = false;
+			e.hasLandmarkEndpoint = landmarkTag;
+			edgesList.push_back(e);
+		}
+		else if(tag == "EDGE_SE3_TRACKXYZ" && v.size() == 13)
+		{
+			EdgeEntry e;
+			e.from = atoi(v[1].c_str());
+			e.to   = atoi(v[2].c_str());
+			// v[3] = param_offset id, ignored
+			e.transform = Transform(uStr2Float(v[4]), uStr2Float(v[5]), uStr2Float(v[6]), 0, 0, 0);
+			e.info = cv::Mat::eye(6, 6, CV_64FC1);
+			e.info.at<double>(0, 0) = uStr2Double(v[7]);
+			e.info.at<double>(0, 1) = e.info.at<double>(1, 0) = uStr2Double(v[8]);
+			e.info.at<double>(0, 2) = e.info.at<double>(2, 0) = uStr2Double(v[9]);
+			e.info.at<double>(1, 1) = uStr2Double(v[10]);
+			e.info.at<double>(1, 2) = e.info.at<double>(2, 1) = uStr2Double(v[11]);
+			e.info.at<double>(2, 2) = uStr2Double(v[12]);
+			e.type = Link::kLandmark;
+			e.isPrior = false;
+			e.hasLandmarkEndpoint = true;
+			edgesList.push_back(e);
+		}
+		else if(tag == "EDGE_PRIOR_SE2" && v.size() == 11)
+		{
+			EdgeEntry e;
+			e.from = atoi(v[1].c_str());
+			e.to = e.from;
+			e.transform = Transform(uStr2Float(v[2]), uStr2Float(v[3]), uStr2Float(v[4]));
+			e.info = cv::Mat::eye(6, 6, CV_64FC1);
+			e.info.at<double>(0, 0) = uStr2Double(v[5]);
+			e.info.at<double>(0, 1) = e.info.at<double>(1, 0) = uStr2Double(v[6]);
+			e.info.at<double>(0, 5) = e.info.at<double>(5, 0) = uStr2Double(v[7]);
+			e.info.at<double>(1, 1) = uStr2Double(v[8]);
+			e.info.at<double>(1, 5) = e.info.at<double>(5, 1) = uStr2Double(v[9]);
+			e.info.at<double>(5, 5) = uStr2Double(v[10]);
+			e.type = Link::kPosePrior;
+			e.isPrior = true;
+			e.hasLandmarkEndpoint = false;
+			edgesList.push_back(e);
+		}
+		else if(tag == "EDGE_PRIOR_SE2_XY" && v.size() == 7)
+		{
+			EdgeEntry e;
+			e.from = atoi(v[1].c_str());
+			e.to = e.from;
+			e.transform = Transform(uStr2Float(v[2]), uStr2Float(v[3]), 0);
+			e.info = cv::Mat::eye(6, 6, CV_64FC1);
+			e.info.at<double>(0, 0) = uStr2Double(v[4]);
+			e.info.at<double>(0, 1) = e.info.at<double>(1, 0) = uStr2Double(v[5]);
+			e.info.at<double>(1, 1) = uStr2Double(v[6]);
+			// no orientation info on this prior
+			e.info.at<double>(3, 3) = e.info.at<double>(4, 4) = e.info.at<double>(5, 5) = 1.0 / 9999.0;
+			e.type = Link::kPosePrior;
+			e.isPrior = true;
+			e.hasLandmarkEndpoint = false;
+			edgesList.push_back(e);
+		}
+		else if(tag == "EDGE_SE3_PRIOR" && v.size() == 31)
+		{
+			EdgeEntry e;
+			e.from = atoi(v[1].c_str());
+			e.to = e.from;
+			// v[2] = param_offset id, ignored
+			e.transform = Transform(uStr2Float(v[3]), uStr2Float(v[4]), uStr2Float(v[5]),
+									uStr2Float(v[6]), uStr2Float(v[7]), uStr2Float(v[8]), uStr2Float(v[9]));
+			e.info = cv::Mat::eye(6, 6, CV_64FC1);
+			int idx = 10;
+			for(int r = 0; r < 6; ++r)
+			{
+				for(int c = r; c < 6; ++c)
+				{
+					e.info.at<double>(r, c) = uStr2Double(v[idx++]);
+					if(r != c) e.info.at<double>(c, r) = e.info.at<double>(r, c);
+				}
+			}
+			e.type = Link::kPosePrior;
+			e.isPrior = true;
+			e.hasLandmarkEndpoint = false;
+			edgesList.push_back(e);
+		}
+		else if(tag == "EDGE_POINTXYZ_PRIOR" && v.size() == 11)
+		{
+			EdgeEntry e;
+			e.from = atoi(v[1].c_str());
+			e.to = e.from;
+			e.transform = Transform(uStr2Float(v[2]), uStr2Float(v[3]), uStr2Float(v[4]), 0, 0, 0);
+			e.info = cv::Mat::eye(6, 6, CV_64FC1);
+			e.info.at<double>(0, 0) = uStr2Double(v[5]);
+			e.info.at<double>(0, 1) = e.info.at<double>(1, 0) = uStr2Double(v[6]);
+			e.info.at<double>(0, 2) = e.info.at<double>(2, 0) = uStr2Double(v[7]);
+			e.info.at<double>(1, 1) = uStr2Double(v[8]);
+			e.info.at<double>(1, 2) = e.info.at<double>(2, 1) = uStr2Double(v[9]);
+			e.info.at<double>(2, 2) = uStr2Double(v[10]);
+			// no orientation info on this prior
+			e.info.at<double>(3, 3) = e.info.at<double>(4, 4) = e.info.at<double>(5, 5) = 1.0 / 9999.0;
+			e.type = Link::kPosePrior;
+			e.isPrior = true;
+			e.hasLandmarkEndpoint = false;
+			edgesList.push_back(e);
+		}
+		else if(tag == "EDGE_SE2_SWITCHABLE" && v.size() == 13)
+		{
+			EdgeEntry e;
+			e.from = atoi(v[1].c_str());
+			e.to   = atoi(v[2].c_str());
+			// v[3] = switch vertex id, ignored
+			e.transform = Transform(uStr2Float(v[4]), uStr2Float(v[5]), uStr2Float(v[6]));
+			e.info = cv::Mat::eye(6, 6, CV_64FC1);
+			e.info.at<double>(0, 0) = uStr2Double(v[7]);
+			e.info.at<double>(0, 1) = e.info.at<double>(1, 0) = uStr2Double(v[8]);
+			e.info.at<double>(0, 5) = e.info.at<double>(5, 0) = uStr2Double(v[9]);
+			e.info.at<double>(1, 1) = uStr2Double(v[10]);
+			e.info.at<double>(1, 5) = e.info.at<double>(5, 1) = uStr2Double(v[11]);
+			e.info.at<double>(5, 5) = uStr2Double(v[12]);
+			e.type = Link::kUndef;
+			e.isPrior = false;
+			e.hasLandmarkEndpoint = false;
+			edgesList.push_back(e);
+		}
+		else if(tag == "EDGE_SE3_SWITCHABLE" && v.size() == 32)
+		{
+			EdgeEntry e;
+			e.from = atoi(v[1].c_str());
+			e.to   = atoi(v[2].c_str());
+			// v[3] = switch vertex id, ignored
+			e.transform = Transform(uStr2Float(v[4]), uStr2Float(v[5]), uStr2Float(v[6]),
+									uStr2Float(v[7]), uStr2Float(v[8]), uStr2Float(v[9]), uStr2Float(v[10]));
+			e.info = cv::Mat::eye(6, 6, CV_64FC1);
+			int idx = 11;
+			for(int r = 0; r < 6; ++r)
+			{
+				for(int c = r; c < 6; ++c)
+				{
+					e.info.at<double>(r, c) = uStr2Double(v[idx++]);
+					if(r != c) e.info.at<double>(c, r) = e.info.at<double>(r, c);
+				}
+			}
+			e.type = Link::kUndef;
+			e.isPrior = false;
+			e.hasLandmarkEndpoint = false;
+			edgesList.push_back(e);
+		}
+		else
+		{
+			UWARN("Unsupported or malformed g2o line: \"%s\" (tag=%s, tokens=%d)", line, tag.c_str(), (int)v.size());
+		}
+	}
+	fclose(file);
+
+	// Recover landmarkOffset from vertex order:
+	//   file order = [landmarks with DESCENDING file_ids] + [regular poses with ASCENDING file_ids]
+	// Walk backwards from the end and take the longest ascending suffix as the regular poses.
+	// landmarkOffset = max regular pose id (= last fileId of that suffix).
+	int landmarkOffset = 0;
+	int firstRegularIdx = (int)verticesList.size();
+	if(!verticesList.empty())
+	{
+		firstRegularIdx = (int)verticesList.size() - 1;
+		while(firstRegularIdx > 0 &&
+			  verticesList[firstRegularIdx - 1].fileId < verticesList[firstRegularIdx].fileId)
+		{
+			--firstRegularIdx;
+		}
+		landmarkOffset = verticesList.back().fileId;
+
+		// If the alleged regular suffix actually starts on a definite landmark
+		// (VERTEX_XY / VERTEX_TRACKXYZ), then there are no regular poses and
+		// saveGraph used landmarkOffset = 0; restore that case.
+		if(verticesList[firstRegularIdx].definitelyLandmark)
+		{
+			landmarkOffset = 0;
+			firstRegularIdx = (int)verticesList.size();
+		}
+	}
+
+	// Insert vertices into poses, remapping landmark file ids back to negative.
+	for(int i = 0; i < (int)verticesList.size(); ++i)
+	{
+		int originalId;
+		if(i < firstRegularIdx)
+		{
+			originalId = landmarkOffset - verticesList[i].fileId; // negative
+		}
+		else
+		{
+			originalId = verticesList[i].fileId;
+		}
+		if(poses.find(originalId) == poses.end())
+		{
+			poses.insert(std::make_pair(originalId, verticesList[i].transform));
+		}
+		else
+		{
+			UWARN("Vertex %d (file id %d) already exists, ignoring duplicate", originalId, verticesList[i].fileId);
+		}
+	}
+
+	// Remap edge endpoints. Any file id > landmarkOffset (or, if landmarkOffset == 0
+	// and there are any landmarks at all, any id present in the landmark prefix)
+	// is a landmark and gets the negative id back.
+	bool allLandmarks = (landmarkOffset == 0 && firstRegularIdx == (int)verticesList.size() && !verticesList.empty());
+	auto remap = [&](int fileId) -> int {
+		if(landmarkOffset > 0 && fileId > landmarkOffset)
+		{
+			return landmarkOffset - fileId; // negative
+		}
+		if(allLandmarks)
+		{
+			return -fileId;
+		}
+		return fileId;
+	};
+
+	for(const EdgeEntry & e : edgesList)
+	{
+		int from = remap(e.from);
+		int to   = e.isPrior ? from : remap(e.to);
+		Link::Type type = e.type;
+		// Promote ambiguous edges (EDGE_SE2) to kLandmark when an endpoint
+		// turns out to be a landmark after remapping.
+		if(type == Link::kUndef && (from < 0 || to < 0))
+		{
+			type = Link::kLandmark;
+		}
+		edgeConstraints.insert(std::make_pair(from, Link(from, to, type, e.transform, e.info)));
+	}
+
+	UINFO("Graph loaded from %s (%d poses, %d edges, landmarkOffset=%d)",
+		  fileName.c_str(), (int)poses.size(), (int)edgeConstraints.size(), landmarkOffset);
+	return true;
+}
+
 bool OptimizerG2O::saveGraph(
 		const std::string & fileName,
 		const std::map<int, Transform> & poses,

corelib/src/optimizer/OptimizerGTSAM.cpp

@@ -239,6 +239,7 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
 					isam2_ = new gtsam::ISAM2(params);
 					addedPoses_.clear();
 					lastAddedConstraints_.clear();
+					isLandmarkWithRotation_.clear();
 					lastRootFactorIndex_.first = 0;
 					lastSwitchId_ = 1000000000;
 				}
@@ -308,7 +309,13 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
 		UDEBUG("fill poses to gtsam... rootId=%d (priorsIgnored=%d landmarksIgnored=%d)",
 				rootId, priorsIgnored()?1:0, landmarksIgnored()?1:0);
 		gtsam::Values initialEstimate;
-		std::map<int, bool> isLandmarkWithRotation;
+		// In batch (non-iSAM2) mode each optimize() call is independent.
+		// In iSAM2 mode the map persists so we can resolve landmarks added
+		// in a previous incremental call but referenced by a new edge.
+		if(!isam2_)
+		{
+			isLandmarkWithRotation_.clear();
+		}
 		for(std::map<int, Transform>::const_iterator iter = newPoses.begin(); iter!=newPoses.end(); ++iter)
 		{
 			UASSERT(!iter->second.isNull());
@@ -328,12 +335,12 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
 					if (1 / static_cast<double>(jter->second.infMatrix().at<double>(5,5)) >= 9999.0)
 					{
 						initialEstimate.insert(iter->first, gtsam::Point2(iter->second.x(), iter->second.y()));
-						isLandmarkWithRotation.insert(std::make_pair(iter->first, false));
+						isLandmarkWithRotation_.insert(std::make_pair(iter->first, false));
 					}
 					else
 					{
 						initialEstimate.insert(iter->first, gtsam::Pose2(iter->second.x(), iter->second.y(), iter->second.theta()));
-						isLandmarkWithRotation.insert(std::make_pair(iter->first, true));
+						isLandmarkWithRotation_.insert(std::make_pair(iter->first, true));
 					}
 					addedPoses_.insert(iter->first);
 				}
@@ -357,12 +364,12 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
 						1 / static_cast<double>(jter->second.infMatrix().at<double>(5,5)) >= 9999.0)
 					{
 						initialEstimate.insert(iter->first, gtsam::Point3(iter->second.x(), iter->second.y(), iter->second.z()));
-						isLandmarkWithRotation.insert(std::make_pair(iter->first, false));
+						isLandmarkWithRotation_.insert(std::make_pair(iter->first, false));
 					}
 					else
 					{
 						initialEstimate.insert(iter->first, gtsam::Pose3(iter->second.toEigen4d()));
-						isLandmarkWithRotation.insert(std::make_pair(iter->first, true));
+						isLandmarkWithRotation_.insert(std::make_pair(iter->first, true));
 					}
 					addedPoses_.insert(iter->first);
 				}
@@ -390,7 +397,7 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
 				{
 					if(isSlam2d())
 					{
-						if(id1 < 0 && !isLandmarkWithRotation.at(id1))
+						if(id1 < 0 && !isLandmarkWithRotation_.at(id1))
 						{
 							gtsam::noiseModel::Diagonal::shared_ptr model = gtsam::noiseModel::Diagonal::Variances(gtsam::Vector2(
 									1/iter->second.infMatrix().at<double>(0,0),
@@ -429,7 +436,7 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
 					}
 					else
 					{
-						if(id1 < 0 && !isLandmarkWithRotation.at(id1))
+						if(id1 < 0 && !isLandmarkWithRotation_.at(id1))
 						{
 							gtsam::noiseModel::Diagonal::shared_ptr model = gtsam::noiseModel::Diagonal::Precisions(gtsam::Vector3(
 										iter->second.infMatrix().at<double>(0,0),
@@ -501,9 +508,9 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
 						t = iter->second.transform().inverse();
 						std::swap(id1, id2); // should be node -> landmark
 					}
-
+					UASSERT(isLandmarkWithRotation_.find(id2) != isLandmarkWithRotation_.end());
 #ifdef RTABMAP_VERTIGO
-					if(this->isRobust() && isLandmarkWithRotation.at(id2))
+					if(this->isRobust() && isLandmarkWithRotation_.at(id2))
 					{
 						// create new switch variable
 						// Sunderhauf IROS 2012:
@@ -521,7 +528,7 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
 						gtsam::noiseModel::Diagonal::shared_ptr switchPriorModel = gtsam::noiseModel::Diagonal::Sigmas(gtsam::Vector1(1.0));
 						graph.add(gtsam::PriorFactor<vertigo::SwitchVariableLinear> (gtsam::Symbol('s',lastSwitchId_), vertigo::SwitchVariableLinear(prior), switchPriorModel));
 					}
-					else if(this->isRobust() && !isLandmarkWithRotation.at(id2))
+					else if(this->isRobust() && !isLandmarkWithRotation_.at(id2))
 					{
 						UWARN("%s cannot be used for landmark constraints without orientation.", Parameters::kOptimizerRobust().c_str());
 					}
@@ -529,7 +536,7 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
 
 					if(isSlam2d())
 					{
-						if(isLandmarkWithRotation.at(id2))
+						if(isLandmarkWithRotation_.at(id2))
 						{
 							Eigen::Matrix<double, 3, 3> information = Eigen::Matrix<double, 3, 3>::Identity();
 							if(!isCovarianceIgnored())
@@ -592,7 +599,7 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
 					}
 					else
 					{
-						if(isLandmarkWithRotation.at(id2))
+						if(isLandmarkWithRotation_.at(id2))
 						{
 							Eigen::Matrix<double, 6, 6> information = Eigen::Matrix<double, 6, 6>::Identity();
 							if(!isCovarianceIgnored())
@@ -807,9 +814,9 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
 								gtsam::Pose2 p = iter->value.cast<gtsam::Pose2>();
 								tmpPoses.insert(std::make_pair(key, Transform(p.x(), p.y(), p.theta())));
 							}
-							else if(!landmarksIgnored() && isLandmarkWithRotation.find(key)!=isLandmarkWithRotation.end())
+							else if(!landmarksIgnored() && isLandmarkWithRotation_.find(key)!=isLandmarkWithRotation_.end())
 							{
-								if(isLandmarkWithRotation.at(key))
+								if(isLandmarkWithRotation_.at(key))
 								{
 									newPoses.at(key).getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
 									gtsam::Pose2 p = iter->value.cast<gtsam::Pose2>();
@@ -830,9 +837,9 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
 								gtsam::Pose3 p = iter->value.cast<gtsam::Pose3>();
 								tmpPoses.insert(std::make_pair(key, Transform::fromEigen4d(p.matrix())));
 							}
-							else if(!landmarksIgnored() && isLandmarkWithRotation.find(key)!=isLandmarkWithRotation.end())
+							else if(!landmarksIgnored() && isLandmarkWithRotation_.find(key)!=isLandmarkWithRotation_.end())
 							{
-								if(isLandmarkWithRotation.at(key))
+								if(isLandmarkWithRotation_.at(key))
 								{
 									gtsam::Pose3 p = iter->value.cast<gtsam::Pose3>();
 									tmpPoses.insert(std::make_pair(key, Transform::fromEigen4d(p.matrix())));
@@ -1000,9 +1007,9 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
 						gtsam::Pose2 p = iter->value.cast<gtsam::Pose2>();
 						optimizedPoses.insert(std::make_pair(key, Transform(p.x(), p.y(), z, roll, pitch, p.theta())));
 					}
-					else if(!landmarksIgnored() && isLandmarkWithRotation.find(key)!=isLandmarkWithRotation.end())
+					else if(!landmarksIgnored() && isLandmarkWithRotation_.find(key)!=isLandmarkWithRotation_.end())
 					{
-						if(isLandmarkWithRotation.at(key))
+						if(isLandmarkWithRotation_.at(key))
 						{
 							poses.at(key).getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
 							gtsam::Pose2 p = iter->value.cast<gtsam::Pose2>();
@@ -1023,9 +1030,9 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
 						gtsam::Pose3 p = iter->value.cast<gtsam::Pose3>();
 						optimizedPoses.insert(std::make_pair(key, Transform::fromEigen4d(p.matrix())));
 					}
-					else if(!landmarksIgnored() && isLandmarkWithRotation.find(key)!=isLandmarkWithRotation.end())
+					else if(!landmarksIgnored() && isLandmarkWithRotation_.find(key)!=isLandmarkWithRotation_.end())
 					{
-						if(isLandmarkWithRotation.at(key))
+						if(isLandmarkWithRotation_.at(key))
 						{
 							gtsam::Pose3 p = iter->value.cast<gtsam::Pose3>();
 							optimizedPoses.insert(std::make_pair(key, Transform::fromEigen4d(p.matrix())));

examples/WifiMapping/WifiThread.h

@@ -177,7 +177,8 @@ private:
 			struct iwreq req;
 			struct iw_statistics stats;
 
-			strncpy(req.ifr_name, interfaceName_.c_str(), IFNAMSIZ);
+			strncpy(req.ifr_name, interfaceName_.c_str(), IFNAMSIZ - 1);
+			req.ifr_name[IFNAMSIZ - 1] = '\0';
 
 			//make room for the iw_statistics object
 			req.u.data.pointer = (caddr_t) &stats;

guilib/include/rtabmap/gui/CameraViewer.h

@@ -77,6 +77,7 @@ private:
 	QElapsedTimer fpsTimer_;
 	double lastCapturePeriod_;
 	double previousCaptureStamp_;
+	std::map<int, float> _landmarksSize;
 };
 
 } /* namespace rtabmap */

guilib/include/rtabmap/gui/PreferencesDialog.h

@@ -364,6 +364,7 @@ private Q_SLOTS:
 	void updateStereoDisparityVisibility();
 	void updateFeatureMatchingVisibility();
 	void updateGlobalDescriptorVisibility();
+	void updateAvailableMarkerDictionaries();
 	void updateOdometryStackedIndex(int index);
 	void useOdomFeatures();
 	void changeWorkingDirectory();

guilib/src/AboutDialog.cpp

@@ -107,6 +107,13 @@ AboutDialog::AboutDialog(QWidget * parent) :
 	_ui->label_fastcv->setText("No");
 	_ui->label_fastcv_license->setEnabled(false);
 #endif
+#ifdef RTABMAP_APRILTAG
+	_ui->label_apriltag->setText("Yes");
+	_ui->label_apriltag_license->setEnabled(true);
+#else
+	_ui->label_apriltag->setText("No");
+	_ui->label_apriltag_license->setEnabled(false);
+#endif
 #ifdef RTABMAP_PDAL
 	_ui->label_pdal->setText("Yes");
 	_ui->label_pdal_license->setEnabled(true);

guilib/src/CameraViewer.cpp

@@ -36,6 +36,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/gui/CloudViewer.h>
 #include <rtabmap/utilite/UCv2Qt.h>
 #include <rtabmap/utilite/ULogger.h>
+#include <rtabmap/utilite/UTimer.h>
 #include <QtCore/QMetaType>
 #include <QHBoxLayout>
 #include <QVBoxLayout>
@@ -84,7 +85,7 @@ CameraViewer::CameraViewer(QWidget * parent, const ParametersMap & parameters) :
 	showScanCheckbox_->setChecked(true);
 
 	markerCheckbox_ = new QCheckBox("Detect markers", this);
-#ifdef HAVE_OPENCV_ARUCO
+#if defined(HAVE_OPENCV_ARUCO) || defined(RTABMAP_APRILTAG)
 	markerCheckbox_->setEnabled(true);
 	markerDetector_ = new MarkerDetector(parameters);
 #else
@@ -151,6 +152,7 @@ void CameraViewer::showImage(const rtabmap::SensorData & data)
 
 	imageView_->setVisible(!left.empty() || !left.empty());
 	std::map<int, MarkerInfo> detections;
+	UTimer markerDetectionTime;
 	if(!left.empty())
 	{
 		std::vector<CameraModel> models;
@@ -165,12 +167,22 @@ void CameraViewer::showImage(const rtabmap::SensorData & data)
 				}
 			}
 		}
+		else
+		{
+			_landmarksSize.clear();
+		}
 			
 		if(!models.empty() && models[0].isValidForProjection())
 		{
 			cv::Mat imageWithDetections;
-			detections = markerDetector_->detect(left, models, depthOrRight, std::map<int, float>(), &imageWithDetections);
+			detections = markerDetector_->detect(left, models, depthOrRight, _landmarksSize, &imageWithDetections);
 			imageView_->setImage(uCvMat2QImage(imageWithDetections));
+			for(std::map<int, MarkerInfo>::iterator iter=detections.begin(); iter!=detections.end(); ++iter)
+			{
+				if(iter->second.length() > 0.0f) {
+					_landmarksSize.insert(std::make_pair(iter->first, iter->second.length()));
+				}
+			}
 		}
 		else
 		{
@@ -184,6 +196,10 @@ void CameraViewer::showImage(const rtabmap::SensorData & data)
 		sizes.append(QString(" Depth=%1x%2").arg(depthOrRight.cols).arg(depthOrRight.rows));
 	}
 	sizes.append(QString(" FPS capture=%1 render=%2").arg(lastCapturePeriod_>0.0?(int)round(1.0/lastCapturePeriod_):0).arg((int)round(1.0/fpsTimer_.restart()*1000)));
+	if(markerCheckbox_->isEnabled() && markerCheckbox_->isChecked())
+	{
+		sizes.append(QString(" Marker=%1ms").arg(int(markerDetectionTime.ticks()*1000)));
+	}
 	imageSizeLabel_->setText(sizes);
 
 	if(!depthOrRight.empty() &&
@@ -236,7 +252,7 @@ void CameraViewer::showImage(const rtabmap::SensorData & data)
 #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);
+					std::string num = uFormat("%d (%.1f cm)", iter->first, iter->second.length()*100.0f);
 					cloudView_->addOrUpdateText(
 							std::string("landmark_str_") + num,
 							num,

guilib/src/MainWindow.cpp

@@ -5239,6 +5239,7 @@ void MainWindow::drawKeypoints(const std::multimap<int, cv::KeyPoint> & refWords
 
 void MainWindow::drawLandmarks(cv::Mat & image, const Signature & signature)
 {
+	UDEBUG("%ld landmarks", signature.getLandmarks().size());
 	for(std::map<int, Link>::const_iterator iter=signature.getLandmarks().begin(); iter!=signature.getLandmarks().end(); ++iter)
 	{
 		// Project in all cameras in which the landmark is visible
@@ -5295,6 +5296,9 @@ void MainWindow::drawLandmarks(cv::Mat & image, const Signature & signature)
 							{
 								imagePoints[j].x += i*model.imageWidth();
 							}
+							// Make sure the frame origin is visible
+							valid = imagePoints[0].x >= i*model.imageWidth() && imagePoints[0].x < (i+1)*model.imageWidth() &&
+									imagePoints[0].y >= 0 && imagePoints[0].y < image.rows;
 						}
 					}
 					if(valid)

guilib/src/PreferencesDialog.cpp

@@ -475,15 +475,14 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	_ui->checkBox_showOdomFrustums->setChecked(false);
 #endif
 
-	//if OpenCV < 3.4.2
+#if !defined(HAVE_OPENCV_ARUCO) && !defined(RTABMAP_APRILTAG)
-#if CV_MAJOR_VERSION < 3 || (CV_MAJOR_VERSION == 3 && (CV_MINOR_VERSION <4 || (CV_MINOR_VERSION ==4 && CV_SUBMINOR_VERSION<2)))
+	_ui->label_markerDetection->setText(_ui->label_markerDetection->text()+" This option works only if OpenCV has been built with \"aruco\" module and/or RTAB-Map has been built with AprilTag library support.");
-	_ui->ArucoDictionary->setItemData(17, 0, Qt::UserRole - 1);
-	_ui->ArucoDictionary->setItemData(18, 0, Qt::UserRole - 1);
-	_ui->ArucoDictionary->setItemData(19, 0, Qt::UserRole - 1);
-	_ui->ArucoDictionary->setItemData(20, 0, Qt::UserRole - 1);
 #endif
 #ifndef HAVE_OPENCV_ARUCO
-	_ui->label_markerDetection->setText(_ui->label_markerDetection->text()+" This option works only if OpenCV has been built with \"aruco\" module.");
+	_ui->MarkerStrategy->setItemData(0, 0, Qt::UserRole - 1);
+#endif
+#ifndef RTABMAP_APRILTAG
+	_ui->MarkerStrategy->setItemData(1, 0, Qt::UserRole - 1);
 #endif
 
 #ifndef RTABMAP_MADGWICK
@@ -1751,18 +1750,30 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	_ui->stereosgbm_mode->setObjectName(Parameters::kStereoSGBMMode().c_str());
 
 	// Aruco marker
-	_ui->ArucoDictionary->setObjectName(Parameters::kMarkerDictionary().c_str());
+	_ui->MarkerStrategy->setObjectName(Parameters::kMarkerStrategy().c_str());
-	_ui->ArucoMarkerLength->setObjectName(Parameters::kMarkerLength().c_str());
+	connect(_ui->MarkerStrategy, SIGNAL(currentIndexChanged(int)), _ui->stackedWidget_markerStrategy, SLOT(setCurrentIndex(int)));
-	_ui->ArucoMaxDepthError->setObjectName(Parameters::kMarkerMaxDepthError().c_str());
+	connect(_ui->MarkerStrategy, SIGNAL(currentIndexChanged(int)), this, SLOT(updateAvailableMarkerDictionaries()));
-	_ui->ArucoVarianceLinear->setObjectName(Parameters::kMarkerVarianceLinear().c_str());
+	_ui->MarkerStrategy->setCurrentIndex(Parameters::defaultMarkerStrategy());
-	_ui->ArucoVarianceAngular->setObjectName(Parameters::kMarkerVarianceAngular().c_str());
+	updateAvailableMarkerDictionaries();
-	_ui->ArucoVarianceOrientationIgnored->setObjectName(Parameters::kMarkerVarianceOrientationIgnored().c_str());
+	_ui->MarkerDictionary->setObjectName(Parameters::kMarkerDictionary().c_str());
-	_ui->ArucoMarkerRangeMin->setObjectName(Parameters::kMarkerMinRange().c_str());
+	_ui->MarkerLength->setObjectName(Parameters::kMarkerLength().c_str());
-	_ui->ArucoMarkerRangeMax->setObjectName(Parameters::kMarkerMaxRange().c_str());
+	_ui->MarkerLengths->setObjectName(Parameters::kMarkerLengths().c_str());
-	_ui->ArucoMarkerPriors->setObjectName(Parameters::kMarkerPriors().c_str());
+	_ui->MarkerMaxDepthError->setObjectName(Parameters::kMarkerMaxDepthError().c_str());
-	_ui->ArucoPriorsVarianceLinear->setObjectName(Parameters::kMarkerPriorsVarianceLinear().c_str());
+	_ui->MarkerVarianceLinear->setObjectName(Parameters::kMarkerVarianceLinear().c_str());
-	_ui->ArucoPriorsVarianceAngular->setObjectName(Parameters::kMarkerPriorsVarianceAngular().c_str());
+	_ui->MarkerVarianceAngular->setObjectName(Parameters::kMarkerVarianceAngular().c_str());
-	_ui->ArucoCornerRefinementMethod->setObjectName(Parameters::kMarkerCornerRefinementMethod().c_str());
+	_ui->MarkerVarianceOrientationIgnored->setObjectName(Parameters::kMarkerVarianceOrientationIgnored().c_str());
+	_ui->MarkerRangeMin->setObjectName(Parameters::kMarkerMinRange().c_str());
+	_ui->MarkerRangeMax->setObjectName(Parameters::kMarkerMaxRange().c_str());
+	_ui->MarkerPriors->setObjectName(Parameters::kMarkerPriors().c_str());
+	_ui->MarkerPriorsVarianceLinear->setObjectName(Parameters::kMarkerPriorsVarianceLinear().c_str());
+	_ui->MarkerPriorsVarianceAngular->setObjectName(Parameters::kMarkerPriorsVarianceAngular().c_str());
+	_ui->OpenCVCornerRefinementMethod->setObjectName(Parameters::kMarkerOpenCVCornerRefinementMethod().c_str());
+	_ui->apriltag_nthreads->setObjectName(Parameters::kMarkerAprilTagNThreads().c_str());
+	_ui->apriltag_quad_decimate->setObjectName(Parameters::kMarkerAprilTagQuadDecimate().c_str());
+	_ui->apriltag_quad_sigma->setObjectName(Parameters::kMarkerAprilTagQuadSigma().c_str());
+	_ui->apriltag_refine_edges->setObjectName(Parameters::kMarkerAprilTagRefineEdges().c_str());
+	_ui->apriltag_decode_sharpening->setObjectName(Parameters::kMarkerAprilTagDecodeSharpening().c_str());
+	_ui->apriltag_debug->setObjectName(Parameters::kMarkerAprilTagDebug().c_str());
 
 	// IMU filter
 	_ui->doubleSpinBox_imuFilterMadgwickGain->setObjectName(Parameters::kImuFilterMadgwickGain().c_str());
@@ -3938,15 +3949,36 @@ bool PreferencesDialog::validateForm()
 		_ui->checkbox_odomDisabled->setChecked(false);
 	}
 
+
+	if(_ui->MarkerStrategy->currentIndex() == 0)
+	{
 #if CV_MAJOR_VERSION < 3 || (CV_MAJOR_VERSION == 3 && (CV_MINOR_VERSION <4 || (CV_MINOR_VERSION ==4 && CV_SUBMINOR_VERSION<2)))
-	if(_ui->ArucoDictionary->currentIndex()>=17)
+		if(_ui->MarkerDictionary->currentIndex()>=17)
 		{
 			QMessageBox::warning(this, tr("Parameter warning"),
-				tr("ArUco dictionary: cannot select AprilTag dictionary, OpenCV version should be at least 3.4.2. Setting back to 0."));
+					tr("opencv-aruco: cannot use the selected dictionary (%1), OpenCV version should be at least 3.4.2. Setting back to 0.").arg(_ui->MarkerDictionary->currentIndex()));
-		_ui->ArucoDictionary->setCurrentIndex(0);
+			_ui->MarkerDictionary->setCurrentIndex(0);
+		}
+#elif CV_MAJOR_VERSION < 4 || (CV_MAJOR_VERSION == 4 && CV_MINOR_VERSION <8)
+		if(_ui->MarkerDictionary->currentIndex()>=21)
+		{
+			QMessageBox::warning(this, tr("Parameter warning"),
+					tr("Opencv Strategy: cannot use selected dictionary (%1), OpenCV version should be at least 4.8.0. Setting back to 0.").arg(_ui->MarkerDictionary->currentIndex()));
+			_ui->MarkerDictionary->setCurrentIndex(0);
 		}
 #endif
-
+	}
+	else if(_ui->MarkerStrategy->currentIndex() == 1)
+	{
+#ifndef RTABMAP_APRILTAG_WITH_ARUCO
+		if(_ui->MarkerDictionary->currentIndex() < 17 || _ui->MarkerDictionary->currentIndex() == 21)
+		{
+			QMessageBox::warning(this, tr("Parameter warning"),
+					tr("AprilTag Strategy: cannot use selected dictionary (%1), AprilTag should be built with aruco support. Setting back to 17.").arg(_ui->MarkerDictionary->currentIndex()));
+			_ui->MarkerDictionary->setCurrentIndex(17);
+		}
+#endif
+	}
 	return true;
 }
 
@@ -5573,6 +5605,64 @@ void PreferencesDialog::updateGlobalDescriptorVisibility()
 	_ui->groupBox_pydescriptor->setVisible(_ui->comboBox_globalDescriptorExtractor->currentIndex() == 1);
 }
 
+void PreferencesDialog::updateAvailableMarkerDictionaries()
+{
+	Qt::ItemFlags enableFlags = Qt::ItemIsEnabled | Qt::ItemIsSelectable;
+	for(int i=0;i<_ui->MarkerDictionary->count();++i) {
+		_ui->MarkerDictionary->setItemData(i, QVariant(static_cast<int>(enableFlags)), Qt::UserRole - 1); 
+	}
+
+	if(_ui->MarkerStrategy->currentIndex() == 1) // AprilTag Strategy is selected
+	{
+		// ARUCO_ORIGINAL not available with AprilTag lib
+		_ui->MarkerDictionary->setItemData(16, 0, Qt::UserRole - 1);
+#ifndef RTABMAP_APRILTAG_WITH_ARUCO
+		// disable all aruco dictionaries
+		for(int i=0;i<17;++i) {
+			_ui->MarkerDictionary->setItemData(i, 0, Qt::UserRole - 1); 
+		}
+		_ui->MarkerDictionary->setItemData(21, 0, Qt::UserRole - 1);
+		if(_ui->MarkerDictionary->currentIndex() < 17 || _ui->MarkerDictionary->currentIndex() > 20)
+		{
+			_ui->MarkerDictionary->setCurrentIndex(20); // 36h11 by default
+		}
+#endif
+	}
+	else //if(_ui->MarkerStrategy->currentIndex() == 0) // OpenCV Strategy is selected
+	{
+		//if OpenCV < 3.4.2
+#if CV_MAJOR_VERSION < 3 || (CV_MAJOR_VERSION == 3 && (CV_MINOR_VERSION <4 || (CV_MINOR_VERSION ==4 && CV_SUBMINOR_VERSION<2)))
+		// disable all apriltag dictionaries
+		for(int i=17;i<21;++i) {
+			_ui->MarkerDictionary->setItemData(i, 0, Qt::UserRole - 1); 
+		}
+		if(_ui->MarkerDictionary->currentIndex() >=17 && _ui->MarkerDictionary->currentIndex() <= 20)
+		{
+			_ui->MarkerDictionary->setCurrentIndex(Parameters::defaultMarkerDictionary());
+		}
+#else
+		if(_ui->MarkerDictionary->currentIndex() >=17 && _ui->MarkerDictionary->currentIndex() <= 20)
+		{
+			// If apriltag is selected, select apriltag refinement by default
+			_ui->OpenCVCornerRefinementMethod->setCurrentIndex(3);
+		}
+		else if(_ui->OpenCVCornerRefinementMethod->currentIndex() == 3)
+		{
+			// If not apriltag dictionary selected, reset refinement to default.
+			_ui->OpenCVCornerRefinementMethod->setCurrentIndex(Parameters::defaultMarkerOpenCVCornerRefinementMethod());
+		}
+#endif
+#if CV_MAJOR_VERSION < 4 || (CV_MAJOR_VERSION == 4 && CV_MINOR_VERSION <8)
+		// disable aruco MPI dictionary	
+		_ui->MarkerDictionary->setItemData(21, 0, Qt::UserRole - 1);
+		if(_ui->MarkerDictionary->currentIndex() == 21)
+		{
+			_ui->MarkerDictionary->setCurrentIndex(Parameters::defaultMarkerDictionary());
+		}
+#endif
+	}
+}
+
 void PreferencesDialog::updateOdometryStackedIndex(int index)
 {
 	if(index == 11) // FLOAM -> LOAM
@@ -6237,7 +6327,7 @@ bool PreferencesDialog::isMarkerDetection() const
 }
 double PreferencesDialog::getMarkerLength() const
 {
-	return _ui->ArucoMarkerLength->value();
+	return _ui->MarkerLength->value();
 }
 bool PreferencesDialog::isCloudMeshing() const
 {

guilib/src/ui/preferencesDialog.ui

@@ -63,7 +63,7 @@
           <property name="geometry">
            <rect>
             <x>0</x>
-            <y>-700</y>
+            <y>0</y>
             <width>684</width>
             <height>5218</height>
            </rect>
@@ -95,7 +95,7 @@
                 <enum>QFrame::Raised</enum>
                </property>
                <property name="currentIndex">
-                <number>8</number>
+                <number>18</number>
                </property>
                <widget class="QWidget" name="page_22">
                 <layout class="QVBoxLayout" name="verticalLayout_29" stretch="0,0">
@@ -15505,7 +15505,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                 </layout>
                </widget>
                <widget class="QWidget" name="page_78">
-                <layout class="QVBoxLayout" name="verticalLayout_137">
+                <layout class="QVBoxLayout" name="verticalLayout_137" stretch="0,1">
                  <item>
                   <widget class="QGroupBox" name="groupBox_markerDetection2">
                    <property name="title">
@@ -15514,30 +15514,29 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                    <layout class="QVBoxLayout" name="verticalLayout_136">
                     <item>
                      <layout class="QGridLayout" name="gridLayout_63" columnstretch="0,1">
-                      <item row="7" column="0">
+                      <item row="1" column="0">
-                       <widget class="QDoubleSpinBox" name="ArucoMarkerRangeMax">
+                       <widget class="QComboBox" name="MarkerStrategy">
-                        <property name="suffix">
+                        <item>
-                         <string> m</string>
+                         <property name="text">
+                          <string>opencv-aruco</string>
                          </property>
-                        <property name="decimals">
+                        </item>
-                         <number>2</number>
+                        <item>
+                         <property name="text">
+                          <string>apriltag</string>
                          </property>
-                        <property name="minimum">
+                        </item>
-                         <double>0.000000000000000</double>
+                       </widget>
-                        </property>
+                      </item>
-                        <property name="maximum">
+                      <item row="8" column="0">
-                         <double>999.000000000000000</double>
+                       <widget class="QCheckBox" name="MarkerVarianceOrientationIgnored">
-                        </property>
+                        <property name="text">
-                        <property name="singleStep">
+                         <string/>
-                         <double>1.000000000000000</double>
-                        </property>
-                        <property name="value">
-                         <double>0.000000000000000</double>
                         </property>
                        </widget>
                       </item>
-                      <item row="4" column="0">
+                      <item row="6" column="0">
-                       <widget class="QDoubleSpinBox" name="ArucoVarianceAngular">
+                       <widget class="QDoubleSpinBox" name="MarkerVarianceLinear">
                         <property name="suffix">
                          <string/>
                         </property>
@@ -15547,9 +15546,6 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                         <property name="minimum">
                          <double>0.000001000000000</double>
                         </property>
-                        <property name="maximum">
-                         <double>9999.000000000000000</double>
-                        </property>
                         <property name="singleStep">
                          <double>0.001000000000000</double>
                         </property>
@@ -15558,8 +15554,8 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                         </property>
                        </widget>
                       </item>
-                      <item row="1" column="0">
+                      <item row="3" column="0">
-                       <widget class="QDoubleSpinBox" name="ArucoMarkerLength">
+                       <widget class="QDoubleSpinBox" name="MarkerLength">
                         <property name="suffix">
                          <string> m</string>
                         </property>
@@ -15577,23 +15573,10 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                         </property>
                        </widget>
                       </item>
-                      <item row="8" column="1">
+                      <item row="11" column="1">
                        <widget class="QLabel" name="label_space2_15">
                         <property name="text">
-                         <string>World prior locations of the markers. The map will be transformed in marker's world frame when a tag is detected. Format is the marker's ID followed by its position (angles in rad), markers are separated by a vertical line (&quot;id1 x y z roll pitch yaw|id2 x y z roll pitch yaw&quot;). Example:  &quot;1 0 0 1 0 0 0|2 1 0 1 0 0 1.57&quot; (marker 2 is 1 meter forward than marker 1 with 90 deg yaw rotation).</string>
+                         <string>World prior locations of the markers. The map will be transformed in marker's world frame when a tag is detected. Format is the marker's ID followed by its position (angles in rad), multiple markers are separated by a vertical line (&quot;id1 x y z roll pitch yaw|id2 x y z roll pitch yaw&quot;). Example:  &quot;1 0 0 1 0 0 0|2 1 0 1 0 0 1.57&quot; (marker 2 is 1 meter forward than marker 1 with 90 deg yaw rotation).</string>
-                        </property>
-                        <property name="wordWrap">
-                         <bool>true</bool>
-                        </property>
-                        <property name="textInteractionFlags">
-                         <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
-                        </property>
-                       </widget>
-                      </item>
-                      <item row="6" column="1">
-                       <widget class="QLabel" name="label_space2_13">
-                        <property name="text">
-                         <string>Minimum detection range (0=disabled).</string>
                         </property>
                         <property name="wordWrap">
                          <bool>true</bool>
@@ -15604,100 +15587,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                        </widget>
                       </item>
                       <item row="10" column="0">
-                       <widget class="QDoubleSpinBox" name="ArucoPriorsVarianceAngular">
+                       <widget class="QDoubleSpinBox" name="MarkerRangeMax">
-                        <property name="suffix">
-                         <string/>
-                        </property>
-                        <property name="decimals">
-                         <number>6</number>
-                        </property>
-                        <property name="minimum">
-                         <double>0.000001000000000</double>
-                        </property>
-                        <property name="maximum">
-                         <double>9999.000000000000000</double>
-                        </property>
-                        <property name="singleStep">
-                         <double>0.001000000000000</double>
-                        </property>
-                        <property name="value">
-                         <double>0.001000000000000</double>
-                        </property>
-                       </widget>
-                      </item>
-                      <item row="9" column="1">
-                       <widget class="QLabel" name="label_space2_16">
-                        <property name="text">
-                         <string>Linear variance to set on marker priors.</string>
-                        </property>
-                        <property name="wordWrap">
-                         <bool>true</bool>
-                        </property>
-                        <property name="textInteractionFlags">
-                         <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
-                        </property>
-                       </widget>
-                      </item>
-                      <item row="10" column="1">
-                       <widget class="QLabel" name="label_space2_17">
-                        <property name="text">
-                         <string>Angular variance to set on marker priors.</string>
-                        </property>
-                        <property name="wordWrap">
-                         <bool>true</bool>
-                        </property>
-                        <property name="textInteractionFlags">
-                         <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
-                        </property>
-                       </widget>
-                      </item>
-                      <item row="0" column="0">
-                       <widget class="QCheckBox" name="RGBDMarkerDetection">
-                        <property name="text">
-                         <string/>
-                        </property>
-                       </widget>
-                      </item>
-                      <item row="3" column="0">
-                       <widget class="QDoubleSpinBox" name="ArucoVarianceLinear">
-                        <property name="suffix">
-                         <string/>
-                        </property>
-                        <property name="decimals">
-                         <number>6</number>
-                        </property>
-                        <property name="minimum">
-                         <double>0.000001000000000</double>
-                        </property>
-                        <property name="singleStep">
-                         <double>0.001000000000000</double>
-                        </property>
-                        <property name="value">
-                         <double>0.001000000000000</double>
-                        </property>
-                       </widget>
-                      </item>
-                      <item row="2" column="0">
-                       <widget class="QDoubleSpinBox" name="ArucoMaxDepthError">
-                        <property name="suffix">
-                         <string> m</string>
-                        </property>
-                        <property name="decimals">
-                         <number>4</number>
-                        </property>
-                        <property name="minimum">
-                         <double>0.000100000000000</double>
-                        </property>
-                        <property name="singleStep">
-                         <double>0.010000000000000</double>
-                        </property>
-                        <property name="value">
-                         <double>0.100000000000000</double>
-                        </property>
-                       </widget>
-                      </item>
-                      <item row="6" column="0">
-                       <widget class="QDoubleSpinBox" name="ArucoMarkerRangeMin">
                         <property name="suffix">
                          <string> m</string>
                         </property>
@@ -15718,33 +15608,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                         </property>
                        </widget>
                       </item>
-                      <item row="2" column="1">
+                      <item row="7" column="1">
-                       <widget class="QLabel" name="label_space2_8">
-                        <property name="text">
-                         <string>Maximum depth error between all corners of a marker when estimating the marker length (when marker length above is 0). The smaller it is, the more perpendicular the camera should be toward the marker to initialize the length.</string>
-                        </property>
-                        <property name="wordWrap">
-                         <bool>true</bool>
-                        </property>
-                        <property name="textInteractionFlags">
-                         <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
-                        </property>
-                       </widget>
-                      </item>
-                      <item row="3" column="1">
-                       <widget class="QLabel" name="label_space2_6">
-                        <property name="text">
-                         <string>Linear variance to set on marker detections. If variance is adjusted to ignore orientation (see below) and Optimizer/Strategy=2 (GTSAM): it is the variance of the range factor, with 9999 to disable range factor and to do only bearing.</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_space2_9">
                         <property name="text">
                          <string>Angular variance to set on marker detections. If variance is adjusted to ignore orientation (see below), the angular variance is ignored with Optimizer/Strategy=1 (g2o) and it corresponds to bearing variance with Optimizer/Strategy=2 (GTSAM).</string>
@@ -15757,8 +15621,38 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                         </property>
                        </widget>
                       </item>
-                      <item row="9" column="0">
+                      <item row="3" column="1">
-                       <widget class="QDoubleSpinBox" name="ArucoPriorsVarianceLinear">
+                       <widget class="QLabel" name="label_space2_7">
+                        <property name="toolTip">
+                         <string>If 0, the length is estimated only on the first marker detected, then re-used for all next detections (i.e., this assumes that markers have all the same length). 
+With &lt;0, the length is estimated once for each unique marker, then re-used for next detections with the same marker ID.</string>
+                        </property>
+                        <property name="text">
+                         <string>Marker length. The length (m) of the markers' side. Value &amp;lt;=0 means automatic marker length estimation using the depth image (the camera should look at the marker perpendicularly for initialization). Mouse over this label to see the difference between negative and 0.</string>
+                        </property>
+                        <property name="wordWrap">
+                         <bool>true</bool>
+                        </property>
+                        <property name="textInteractionFlags">
+                         <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                        </property>
+                       </widget>
+                      </item>
+                      <item row="12" column="1">
+                       <widget class="QLabel" name="label_space2_16">
+                        <property name="text">
+                         <string>Linear variance to set on marker priors.</string>
+                        </property>
+                        <property name="wordWrap">
+                         <bool>true</bool>
+                        </property>
+                        <property name="textInteractionFlags">
+                         <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                        </property>
+                       </widget>
+                      </item>
+                      <item row="13" column="0">
+                       <widget class="QDoubleSpinBox" name="MarkerPriorsVarianceAngular">
                         <property name="suffix">
                          <string/>
                         </property>
@@ -15768,6 +15662,9 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                         <property name="minimum">
                          <double>0.000001000000000</double>
                         </property>
+                        <property name="maximum">
+                         <double>9999.000000000000000</double>
+                        </property>
                         <property name="singleStep">
                          <double>0.001000000000000</double>
                         </property>
@@ -15776,163 +15673,86 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                         </property>
                        </widget>
                       </item>
-                      <item row="7" column="1">
+                      <item row="2" column="0">
-                       <widget class="QLabel" name="label_space2_14">
+                       <widget class="QComboBox" name="MarkerDictionary">
-                        <property name="text">
-                         <string>Maximum detection range (0=unlimited).</string>
-                        </property>
-                        <property name="wordWrap">
-                         <bool>true</bool>
-                        </property>
-                        <property name="textInteractionFlags">
-                         <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
-                        </property>
-                       </widget>
-                      </item>
-                      <item row="8" column="0">
-                       <widget class="QLineEdit" name="ArucoMarkerPriors">
-                        <property name="placeholderText">
-                         <string/>
-                        </property>
-                       </widget>
-                      </item>
-                      <item row="1" column="1">
-                       <widget class="QLabel" name="label_space2_7">
-                        <property name="text">
-                         <string>Marker length. The length (m) of the markers' side. 0 means automatic marker length estimation using the depth image (the camera should look at the marker perpendicularly for initialization).</string>
-                        </property>
-                        <property name="wordWrap">
-                         <bool>true</bool>
-                        </property>
-                        <property name="textInteractionFlags">
-                         <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
-                        </property>
-                       </widget>
-                      </item>
-                      <item row="0" column="1">
-                       <widget class="QLabel" name="label_markerDetection">
-                        <property name="text">
-                         <string>Detect static markers to be added as landmarks for graph optimization. If input data have already landmarks, this will be ignored.</string>
-                        </property>
-                        <property name="wordWrap">
-                         <bool>true</bool>
-                        </property>
-                        <property name="textInteractionFlags">
-                         <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
-                        </property>
-                       </widget>
-                      </item>
-                      <item row="5" column="1">
-                       <widget class="QLabel" name="label_space2_20">
-                        <property name="toolTip">
-                         <string>When this setting is false, the landmark's orientation is optimized during graph optimization. When this setting is true, only the position of the landmark is optimized. This can be useful when the landmark's orientation estimation is not reliable. Note that for Optimizer/Strategy=1 (g2o), only Marker/VarianceLinear needs be set if we ignore orientation. For Optimizer/Strategy=2 (GTSAM), instead of optimizing the landmark's position directly, a bearing/range factor is used, with Marker/VarianceLinear as the variance of the range factor (with 9999 to optimize the position with only a bearing factor) and Marker/VarianceAngular as the variance of the bearing factor (pitch/yaw).</string>
-                        </property>
-                        <property name="text">
-                         <string>Adjust variance to ignore orientation during optimization. Mouseover for more details.</string>
-                        </property>
-                        <property name="wordWrap">
-                         <bool>true</bool>
-                        </property>
-                        <property name="textInteractionFlags">
-                         <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
-                        </property>
-                       </widget>
-                      </item>
-                      <item row="5" column="0">
-                       <widget class="QCheckBox" name="ArucoVarianceOrientationIgnored">
-                        <property name="text">
-                         <string/>
-                        </property>
-                       </widget>
-                      </item>
-                     </layout>
-                    </item>
-                    <item>
-                     <widget class="QGroupBox" name="groupBox_31">
-                      <property name="title">
-                       <string>ArUco</string>
-                      </property>
-                      <layout class="QGridLayout" name="gridLayout_106" columnstretch="0,1">
-                       <item row="0" column="0">
-                        <widget class="QComboBox" name="ArucoDictionary">
                         <item>
                          <property name="text">
-                           <string>4X4_50</string>
+                          <string>ARUCO_4X4_50</string>
                          </property>
                         </item>
                         <item>
                          <property name="text">
-                           <string>4X4_100</string>
+                          <string>ARUCO_4X4_100</string>
                          </property>
                         </item>
                         <item>
                          <property name="text">
-                           <string>4X4_250</string>
+                          <string>ARUCO_4X4_250</string>
                          </property>
                         </item>
                         <item>
                          <property name="text">
-                           <string>4X4_1000</string>
+                          <string>ARUCO_4X4_1000</string>
                          </property>
                         </item>
                         <item>
                          <property name="text">
-                           <string>5X5_50</string>
+                          <string>ARUCO_5X5_50</string>
                          </property>
                         </item>
                         <item>
                          <property name="text">
-                           <string>5X5_100</string>
+                          <string>ARUCO_5X5_100</string>
                          </property>
                         </item>
                         <item>
                          <property name="text">
-                           <string>5X5_250</string>
+                          <string>ARUCO_5X5_250</string>
                          </property>
                         </item>
                         <item>
                          <property name="text">
-                           <string>5X5_1000</string>
+                          <string>ARUCO_5X5_1000</string>
                          </property>
                         </item>
                         <item>
                          <property name="text">
-                           <string>6X6_50</string>
+                          <string>ARUCO_6X6_50</string>
                          </property>
                         </item>
                         <item>
                          <property name="text">
-                           <string>6X6_100</string>
+                          <string>ARUCO_6X6_100</string>
                          </property>
                         </item>
                         <item>
                          <property name="text">
-                           <string>6X6_250</string>
+                          <string>ARUCO_6X6_250</string>
                          </property>
                         </item>
                         <item>
                          <property name="text">
-                           <string>6X6_1000</string>
+                          <string>ARUCO_6X6_1000</string>
                          </property>
                         </item>
                         <item>
                          <property name="text">
-                           <string>7X7_50</string>
+                          <string>ARUCO_7X7_50</string>
                          </property>
                         </item>
                         <item>
                          <property name="text">
-                           <string>7X7_100</string>
+                          <string>ARUCO_7X7_100</string>
                          </property>
                         </item>
                         <item>
                          <property name="text">
-                           <string>7X7_250</string>
+                          <string>ARUCO_7X7_250</string>
                          </property>
                         </item>
                         <item>
                          <property name="text">
-                           <string>7X7_1000</string>
+                          <string>ARUCO_7X7_1000</string>
                          </property>
                         </item>
                         <item>
@@ -15960,9 +15780,14 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                           <string>APRILTAG_36h11</string>
                          </property>
                         </item>
+                        <item>
+                         <property name="text">
+                          <string>ARUCO_MIP_36h12</string>
+                         </property>
+                        </item>
                        </widget>
                       </item>
-                       <item row="0" column="1">
+                      <item row="2" column="1">
                        <widget class="QLabel" name="label_space2_10">
                         <property name="text">
                          <string>Dictionary to use.</string>
@@ -15975,8 +15800,262 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                         </property>
                        </widget>
                       </item>
+                      <item row="0" column="1">
+                       <widget class="QLabel" name="label_markerDetection">
+                        <property name="text">
+                         <string>Detect static markers to be added as landmarks for graph optimization. If input data have already landmarks, this will be ignored.</string>
+                        </property>
+                        <property name="wordWrap">
+                         <bool>true</bool>
+                        </property>
+                        <property name="textInteractionFlags">
+                         <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                        </property>
+                       </widget>
+                      </item>
+                      <item row="6" column="1">
+                       <widget class="QLabel" name="label_space2_6">
+                        <property name="text">
+                         <string>Linear variance to set on marker detections. If variance is adjusted to ignore orientation (see below) and Optimizer/Strategy=2 (GTSAM): it is the variance of the range factor, with 9999 to disable range factor and to do only bearing.</string>
+                        </property>
+                        <property name="wordWrap">
+                         <bool>true</bool>
+                        </property>
+                        <property name="textInteractionFlags">
+                         <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                        </property>
+                       </widget>
+                      </item>
+                      <item row="11" column="0">
+                       <widget class="QLineEdit" name="MarkerPriors">
+                        <property name="placeholderText">
+                         <string/>
+                        </property>
+                       </widget>
+                      </item>
+                      <item row="13" column="1">
+                       <widget class="QLabel" name="label_space2_17">
+                        <property name="text">
+                         <string>Angular variance to set on marker priors.</string>
+                        </property>
+                        <property name="wordWrap">
+                         <bool>true</bool>
+                        </property>
+                        <property name="textInteractionFlags">
+                         <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                        </property>
+                       </widget>
+                      </item>
+                      <item row="8" column="1">
+                       <widget class="QLabel" name="label_space2_20">
+                        <property name="toolTip">
+                         <string>When this setting is false, the landmark's orientation is optimized during graph optimization. When this setting is true, only the position of the landmark is optimized. This can be useful when the landmark's orientation estimation is not reliable. Note that for Optimizer/Strategy=1 (g2o), only Marker/VarianceLinear needs be set if we ignore orientation. For Optimizer/Strategy=2 (GTSAM), instead of optimizing the landmark's position directly, a bearing/range factor is used, with Marker/VarianceLinear as the variance of the range factor (with 9999 to optimize the position with only a bearing factor) and Marker/VarianceAngular as the variance of the bearing factor (pitch/yaw).</string>
+                        </property>
+                        <property name="text">
+                         <string>Adjust variance to ignore orientation during optimization. Mouseover for more details.</string>
+                        </property>
+                        <property name="wordWrap">
+                         <bool>true</bool>
+                        </property>
+                        <property name="textInteractionFlags">
+                         <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                        </property>
+                       </widget>
+                      </item>
+                      <item row="9" column="0">
+                       <widget class="QDoubleSpinBox" name="MarkerRangeMin">
+                        <property name="suffix">
+                         <string> m</string>
+                        </property>
+                        <property name="decimals">
+                         <number>2</number>
+                        </property>
+                        <property name="minimum">
+                         <double>0.000000000000000</double>
+                        </property>
+                        <property name="maximum">
+                         <double>999.000000000000000</double>
+                        </property>
+                        <property name="singleStep">
+                         <double>1.000000000000000</double>
+                        </property>
+                        <property name="value">
+                         <double>0.000000000000000</double>
+                        </property>
+                       </widget>
+                      </item>
+                      <item row="9" column="1">
+                       <widget class="QLabel" name="label_space2_13">
+                        <property name="text">
+                         <string>Minimum detection range (0=disabled).</string>
+                        </property>
+                        <property name="wordWrap">
+                         <bool>true</bool>
+                        </property>
+                        <property name="textInteractionFlags">
+                         <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                        </property>
+                       </widget>
+                      </item>
+                      <item row="10" column="1">
+                       <widget class="QLabel" name="label_space2_14">
+                        <property name="text">
+                         <string>Maximum detection range (0=unlimited).</string>
+                        </property>
+                        <property name="wordWrap">
+                         <bool>true</bool>
+                        </property>
+                        <property name="textInteractionFlags">
+                         <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                        </property>
+                       </widget>
+                      </item>
+                      <item row="5" column="1">
+                       <widget class="QLabel" name="label_space2_8">
+                        <property name="text">
+                         <string>Maximum depth error between all corners of a marker when estimating the marker length (when marker length above is 0). The smaller it is, the more perpendicular the camera should be toward the marker to initialize the length.</string>
+                        </property>
+                        <property name="wordWrap">
+                         <bool>true</bool>
+                        </property>
+                        <property name="textInteractionFlags">
+                         <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                        </property>
+                       </widget>
+                      </item>
+                      <item row="5" column="0">
+                       <widget class="QDoubleSpinBox" name="MarkerMaxDepthError">
+                        <property name="suffix">
+                         <string> m</string>
+                        </property>
+                        <property name="decimals">
+                         <number>4</number>
+                        </property>
+                        <property name="minimum">
+                         <double>0.000100000000000</double>
+                        </property>
+                        <property name="singleStep">
+                         <double>0.010000000000000</double>
+                        </property>
+                        <property name="value">
+                         <double>0.100000000000000</double>
+                        </property>
+                       </widget>
+                      </item>
+                      <item row="0" column="0">
+                       <widget class="QCheckBox" name="RGBDMarkerDetection">
+                        <property name="text">
+                         <string/>
+                        </property>
+                       </widget>
+                      </item>
+                      <item row="12" column="0">
+                       <widget class="QDoubleSpinBox" name="MarkerPriorsVarianceLinear">
+                        <property name="suffix">
+                         <string/>
+                        </property>
+                        <property name="decimals">
+                         <number>6</number>
+                        </property>
+                        <property name="minimum">
+                         <double>0.000001000000000</double>
+                        </property>
+                        <property name="singleStep">
+                         <double>0.001000000000000</double>
+                        </property>
+                        <property name="value">
+                         <double>0.001000000000000</double>
+                        </property>
+                       </widget>
+                      </item>
+                      <item row="1" column="1">
+                       <widget class="QLabel" name="label_space2_24">
+                        <property name="text">
+                         <string>Detector implementation. Note that both aruco and apriltag dictionaries may be used with either strategy. Scroll down to see specific parameters for the strategy used.</string>
+                        </property>
+                        <property name="wordWrap">
+                         <bool>true</bool>
+                        </property>
+                        <property name="textInteractionFlags">
+                         <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                        </property>
+                       </widget>
+                      </item>
+                      <item row="7" column="0">
+                       <widget class="QDoubleSpinBox" name="MarkerVarianceAngular">
+                        <property name="suffix">
+                         <string/>
+                        </property>
+                        <property name="decimals">
+                         <number>6</number>
+                        </property>
+                        <property name="minimum">
+                         <double>0.000001000000000</double>
+                        </property>
+                        <property name="maximum">
+                         <double>9999.000000000000000</double>
+                        </property>
+                        <property name="singleStep">
+                         <double>0.001000000000000</double>
+                        </property>
+                        <property name="value">
+                         <double>0.001000000000000</double>
+                        </property>
+                       </widget>
+                      </item>
+                      <item row="4" column="0">
+                       <widget class="QLineEdit" name="MarkerLengths">
+                        <property name="placeholderText">
+                         <string/>
+                        </property>
+                       </widget>
+                      </item>
+                      <item row="4" column="1">
+                       <widget class="QLabel" name="label_space2_25">
+                        <property name="toolTip">
+                         <string>For example, to detect markers 12 and 14 with lengths of 8 and 15 cm respectively, and all markers between 30 and 40 with a length of 10 cm, set &quot;12 0.08|14 0.15|30:40 0.1&quot;</string>
+                        </property>
+                        <property name="text">
+                         <string>List of markers to detect. Format is the marker's ID followed by its length (in meters), markers are separated by a vertical line (&quot;id1 length|id2 length&quot;). We can also define a range of markers with &quot;id1:id2 length&quot; (id2 included). If empty, all markers of the chosen dictionary can be detected and their length is set/estimated based on Marker Length. Mouse over this label for example.</string>
+                        </property>
+                        <property name="wordWrap">
+                         <bool>true</bool>
+                        </property>
+                        <property name="textInteractionFlags">
+                         <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                        </property>
+                       </widget>
+                      </item>
+                     </layout>
+                    </item>
+                    <item>
+                     <widget class="QStackedWidget" name="stackedWidget_markerStrategy">
+                      <property name="currentIndex">
+                       <number>0</number>
+                      </property>
+                      <widget class="QWidget" name="page_104">
+                       <layout class="QVBoxLayout" name="verticalLayout_189">
+                        <item>
+                         <widget class="QGroupBox" name="groupBox_opencv_aruco">
+                          <property name="title">
+                           <string>OpenCV</string>
+                          </property>
+                          <layout class="QGridLayout" name="gridLayout_106" columnstretch="0,1">
+                           <item row="1" column="1">
+                            <widget class="QLabel" name="label_space2_11">
+                             <property name="text">
+                              <string>Corner refinement method.</string>
+                             </property>
+                             <property name="wordWrap">
+                              <bool>true</bool>
+                             </property>
+                             <property name="textInteractionFlags">
+                              <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                             </property>
+                            </widget>
+                           </item>
                            <item row="1" column="0">
-                        <widget class="QComboBox" name="ArucoCornerRefinementMethod">
+                            <widget class="QComboBox" name="OpenCVCornerRefinementMethod">
                              <item>
                               <property name="text">
                                <string>None</string>
@@ -15999,10 +16078,36 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                              </item>
                             </widget>
                            </item>
-                       <item row="1" column="1">
+                           <item row="2" column="1">
-                        <widget class="QLabel" name="label_space2_11">
+                            <spacer name="verticalSpacer_94">
+                             <property name="orientation">
+                              <enum>Qt::Vertical</enum>
+                             </property>
+                             <property name="sizeHint" stdset="0">
+                              <size>
+                               <width>0</width>
+                               <height>0</height>
+                              </size>
+                             </property>
+                            </spacer>
+                           </item>
+                          </layout>
+                         </widget>
+                        </item>
+                       </layout>
+                      </widget>
+                      <widget class="QWidget" name="page_105">
+                       <layout class="QVBoxLayout" name="verticalLayout_190">
+                        <item>
+                         <widget class="QGroupBox" name="groupBox_apriltag">
+                          <property name="title">
+                           <string>AprilTag</string>
+                          </property>
+                          <layout class="QGridLayout" name="gridLayout_140" columnstretch="0,1">
+                           <item row="4" column="1">
+                            <widget class="QLabel" name="label_space2_28">
                              <property name="text">
-                          <string>Corner refinement method.</string>
+                              <string>decode_sharpening: How much sharpening should be done to decoded images? This can help decode small tags but may or may not help in odd lighting conditions or low light conditions.</string>
                              </property>
                              <property name="wordWrap">
                               <bool>true</bool>
@@ -16012,11 +16117,158 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                              </property>
                             </widget>
                            </item>
+                           <item row="0" column="1">
+                            <widget class="QLabel" name="label_space2_22">
+                             <property name="text">
+                              <string>nthreads: How many threads should be used?</string>
+                             </property>
+                             <property name="wordWrap">
+                              <bool>true</bool>
+                             </property>
+                             <property name="textInteractionFlags">
+                              <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                             </property>
+                            </widget>
+                           </item>
+                           <item row="0" column="0">
+                            <widget class="QSpinBox" name="apriltag_nthreads">
+                             <property name="minimum">
+                              <number>1</number>
+                             </property>
+                             <property name="value">
+                              <number>1</number>
+                             </property>
+                            </widget>
+                           </item>
+                           <item row="1" column="1">
+                            <widget class="QLabel" name="label_space2_23">
+                             <property name="text">
+                              <string>quad_decimate: Detection of quads can be done on a lower-resolution image, improving speed at a cost of pose accuracy and a slight decrease in detection rate. Decoding the binary payload is still done at full resolution.</string>
+                             </property>
+                             <property name="wordWrap">
+                              <bool>true</bool>
+                             </property>
+                             <property name="textInteractionFlags">
+                              <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                             </property>
+                            </widget>
+                           </item>
+                           <item row="3" column="1">
+                            <widget class="QLabel" name="label_space2_27">
+                             <property name="text">
+                              <string>refine_edges: When true, the edges of the each quad are adjusted to &quot;snap to&quot; strong gradients nearby. This is useful when decimation is employed, as it can increase the quality of the initial quad estimate substantially. Generally recommended to be on (true). Very computationally inexpensive. Option is ignored if quad_decimate = 1.</string>
+                             </property>
+                             <property name="wordWrap">
+                              <bool>true</bool>
+                             </property>
+                             <property name="textInteractionFlags">
+                              <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                             </property>
+                            </widget>
+                           </item>
+                           <item row="2" column="1">
+                            <widget class="QLabel" name="label_space2_26">
+                             <property name="text">
+                              <string>quad_sigma: What Gaussian blur should be applied to the segmented image (used for quad detection?)  Parameter is the standard deviation in pixels.  Very noisy images benefit from non-zero values (e.g. 0.8).</string>
+                             </property>
+                             <property name="wordWrap">
+                              <bool>true</bool>
+                             </property>
+                             <property name="textInteractionFlags">
+                              <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                             </property>
+                            </widget>
+                           </item>
+                           <item row="5" column="1">
+                            <widget class="QLabel" name="label_space2_29">
+                             <property name="text">
+                              <string>debug: When true, write a variety of debugging images to the working directory where the app started (not RTAB-Map's working directory) at various stages through the detection process. (Somewhat slow).</string>
+                             </property>
+                             <property name="wordWrap">
+                              <bool>true</bool>
+                             </property>
+                             <property name="textInteractionFlags">
+                              <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                             </property>
+                            </widget>
+                           </item>
+                           <item row="5" column="0">
+                            <widget class="QCheckBox" name="apriltag_debug">
+                             <property name="text">
+                              <string/>
+                             </property>
+                            </widget>
+                           </item>
+                           <item row="3" column="0">
+                            <widget class="QCheckBox" name="apriltag_refine_edges">
+                             <property name="text">
+                              <string/>
+                             </property>
+                            </widget>
+                           </item>
+                           <item row="4" column="0">
+                            <widget class="QDoubleSpinBox" name="apriltag_decode_sharpening">
+                             <property name="suffix">
+                              <string/>
+                             </property>
+                             <property name="decimals">
+                              <number>2</number>
+                             </property>
+                             <property name="minimum">
+                              <double>0.000000000000000</double>
+                             </property>
+                             <property name="singleStep">
+                              <double>0.050000000000000</double>
+                             </property>
+                             <property name="value">
+                              <double>0.250000000000000</double>
+                             </property>
+                            </widget>
+                           </item>
+                           <item row="2" column="0">
+                            <widget class="QDoubleSpinBox" name="apriltag_quad_sigma">
+                             <property name="suffix">
+                              <string/>
+                             </property>
+                             <property name="decimals">
+                              <number>1</number>
+                             </property>
+                             <property name="minimum">
+                              <double>0.000000000000000</double>
+                             </property>
+                             <property name="maximum">
+                              <double>99.900000000000006</double>
+                             </property>
+                             <property name="singleStep">
+                              <double>0.100000000000000</double>
+                             </property>
+                             <property name="value">
+                              <double>0.000000000000000</double>
+                             </property>
+                            </widget>
+                           </item>
+                           <item row="1" column="0">
+                            <widget class="QDoubleSpinBox" name="apriltag_quad_decimate">
+                             <property name="decimals">
+                              <number>1</number>
+                             </property>
+                             <property name="minimum">
+                              <double>1.000000000000000</double>
+                             </property>
+                             <property name="value">
+                              <double>2.000000000000000</double>
+                             </property>
+                            </widget>
+                           </item>
                           </layout>
                          </widget>
                         </item>
                        </layout>
                       </widget>
+                     </widget>
+                    </item>
+                   </layout>
+                  </widget>
                  </item>
                  <item>
                   <spacer name="verticalSpacer_76">

tools/Export/main.cpp

@@ -1978,11 +1978,12 @@ int main(int argc, char * argv[])
 					exportPosesFormat,
 					std::map<int, Transform>(robotPoses.lower_bound(1), robotPoses.end()),
 					links,
-					std::map<int, double>(robotStamps.lower_bound(1), robotStamps.end()));
+					std::map<int, double>(robotStamps.lower_bound(1), robotStamps.end()),
+					parameters);
 			}
 			else
 			{
-				rtabmap::graph::exportPoses(outputPath, exportPosesFormat, robotPoses, links, robotStamps);
+				rtabmap::graph::exportPoses(outputPath, exportPosesFormat, robotPoses, links, robotStamps, parameters);
 			}
 			cv::Vec3f vmin, vmax;
 			graph::computeMinMax(robotPoses, vmin, vmax);
@@ -2001,7 +2002,7 @@ int main(int argc, char * argv[])
 					outputPath = outputDirectory+"/"+baseName+"_camera_poses." + posesExt;
 				else
 					outputPath = outputDirectory+"/"+baseName+"_camera_poses_"+uNumber2Str((int)i)+"." + posesExt;
-				rtabmap::graph::exportPoses(outputPath, exportPosesFormat, cameraPoses[i], std::multimap<int, Link>(), cameraStamps[i]);
+				rtabmap::graph::exportPoses(outputPath, exportPosesFormat, cameraPoses[i], std::multimap<int, Link>(), cameraStamps[i], parameters);
 				cv::Vec3f vmin, vmax;
 				graph::computeMinMax(cameraPoses[i], vmin, vmax);
 				printf("%d camera poses exported to \"%s\". (min=[%f,%f,%f] max=[%f,%f,%f])\n",
@@ -2014,7 +2015,7 @@ int main(int argc, char * argv[])
 		if(exportPosesScan)
 		{
 			std::string outputPath=outputDirectory+"/"+baseName+"_scan_poses." + posesExt;
-			rtabmap::graph::exportPoses(outputPath, exportPosesFormat, scanPoses, std::multimap<int, Link>(), scanStamps);
+			rtabmap::graph::exportPoses(outputPath, exportPosesFormat, scanPoses, std::multimap<int, Link>(), scanStamps, parameters);
 			cv::Vec3f min, max;
 			graph::computeMinMax(scanPoses, min, max);
 			printf("%d scan poses exported to \"%s\". (min=[%f,%f,%f] max=[%f,%f,%f])\n",
@@ -2026,7 +2027,7 @@ int main(int argc, char * argv[])
 		if(exportPosesScan)
 		{
 			std::string outputPath=outputDirectory+"/"+baseName+"_scan_poses." + posesExt;
-			rtabmap::graph::exportPoses(outputPath, exportPosesFormat, scanPoses, std::multimap<int, Link>(), scanStamps);
+			rtabmap::graph::exportPoses(outputPath, exportPosesFormat, scanPoses, std::multimap<int, Link>(), scanStamps, parameters);
 			cv::Vec3f min, max;
 			graph::computeMinMax(scanPoses, min, max);
 			printf("%d scan poses exported to \"%s\". (min=[%f,%f,%f] max=[%f,%f,%f])\n",
@@ -2038,7 +2039,7 @@ int main(int argc, char * argv[])
 		if(exportPosesLandmarks)
 		{
 			std::string outputPath=outputDirectory+"/"+baseName+"_landmark_poses." + posesExt;
-			rtabmap::graph::exportPoses(outputPath, exportPosesFormat, landmarkPoses, std::multimap<int, Link>(), landmarkStamps);
+			rtabmap::graph::exportPoses(outputPath, exportPosesFormat, landmarkPoses, std::multimap<int, Link>(), landmarkStamps, parameters);
 			cv::Vec3f min, max;
 			graph::computeMinMax(landmarkPoses, min, max);
 			printf("%d landmark poses exported to \"%s\". (min=[%f,%f,%f] max=[%f,%f,%f])\n",
@@ -2050,7 +2051,7 @@ int main(int argc, char * argv[])
 		if(exportPosesGps)
 		{
 			std::string outputPath=outputDirectory+"/"+baseName+"_gps_poses." + posesExt;
-			rtabmap::graph::exportPoses(outputPath, exportPosesFormat, gpsPoses, std::multimap<int, Link>(), gpsStamps);
+			rtabmap::graph::exportPoses(outputPath, exportPosesFormat, gpsPoses, std::multimap<int, Link>(), gpsStamps, parameters);
 			printf("%d GPS poses exported to \"%s\".\n",
 					(int)gpsPoses.size(),
 					outputPath.c_str());
@@ -2058,7 +2059,7 @@ int main(int argc, char * argv[])
 		if(exportPosesGt)
 		{
 			std::string outputPath=outputDirectory+"/"+baseName+"_gt_poses." + posesExt;
-			rtabmap::graph::exportPoses(outputPath, exportPosesFormat, gtPoses, std::multimap<int, Link>(), gtStamps);
+			rtabmap::graph::exportPoses(outputPath, exportPosesFormat, gtPoses, std::multimap<int, Link>(), gtStamps, parameters);
 			printf("%d scan poses exported to \"%s\".\n",
 					(int)gtPoses.size(),
 					outputPath.c_str());