diff --git a/CMakeLists.txt b/CMakeLists.txt index 7844ae1c76..60986d09ff 100644 --- a/CMakeLists.txt +++ b/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}) @@ -230,6 +230,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() @@ -866,6 +867,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) @@ -1066,6 +1081,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) @@ -1475,28 +1496,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() @@ -1571,6 +1597,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) diff --git a/Version.h.in b/Version.h.in index 381dcaa37f..2bb9d46af1 100644 --- a/Version.h.in +++ b/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 diff --git a/corelib/include/rtabmap/core/MarkerDetector.h b/corelib/include/rtabmap/core/MarkerDetector.h index d628ca022d..630bf87f3e 100644 --- a/corelib/include/rtabmap/core/MarkerDetector.h +++ b/corelib/include/rtabmap/core/MarkerDetector.h @@ -57,6 +57,12 @@ class MarkerInfo { }; class RTABMAP_CORE_EXPORT MarkerDetector { + +public: + enum Strategy { + kStrategyOpencv, + kStrategyApriltag + }; public: MarkerDetector(const ParametersMap & parameters = ParametersMap()); @@ -84,15 +90,19 @@ class RTABMAP_CORE_EXPORT MarkerDetector { cv::Mat * imageWithDetections = 0); private: -#ifdef HAVE_OPENCV_ARUCO - cv::Ptr detectorParams_; - float markerLength_; + Strategy strategy_; + float markerLength_; + std::map markerLengths_; float maxDepthError_; float maxRange_; float minRange_; int dictionaryId_; +#ifdef HAVE_OPENCV_ARUCO + cv::Ptr detectorParams_; cv::Ptr dictionary_; #endif + void * apriltagLibDetector_; + void * apriltagLibFamily_; }; } /* namespace rtabmap */ diff --git a/corelib/include/rtabmap/core/Parameters.h b/corelib/include/rtabmap/core/Parameters.h index 23cace9e43..3944acf6c5 100644 --- a/corelib/include/rtabmap/core/Parameters.h +++ b/corelib/include/rtabmap/core/Parameters.h @@ -917,19 +917,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, 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, Strategy, int, 0, "Marker detection implementation: 0=OpenCV, 1=AprilTag"); + 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]."); diff --git a/corelib/include/rtabmap/core/optimizer/OptimizerG2O.h b/corelib/include/rtabmap/core/optimizer/OptimizerG2O.h index 30117d6e56..d8c952795f 100644 --- a/corelib/include/rtabmap/core/optimizer/OptimizerG2O.h +++ b/corelib/include/rtabmap/core/optimizer/OptimizerG2O.h @@ -41,6 +41,12 @@ class RTABMAP_CORE_EXPORT OptimizerG2O : public Optimizer static bool isCSparseAvailable(); static bool isCholmodAvailable(); +public: + static bool loadGraph( + const std::string & fileName, + std::map & poses, + std::multimap & edgeConstraints); + public: OptimizerG2O(const ParametersMap & parameters = ParametersMap()); virtual ~OptimizerG2O() {} diff --git a/corelib/include/rtabmap/core/optimizer/OptimizerGTSAM.h b/corelib/include/rtabmap/core/optimizer/OptimizerGTSAM.h index 2d3eb6ba01..c2b5a0f3fd 100644 --- a/corelib/include/rtabmap/core/optimizer/OptimizerGTSAM.h +++ b/corelib/include/rtabmap/core/optimizer/OptimizerGTSAM.h @@ -77,6 +77,7 @@ class RTABMAP_CORE_EXPORT OptimizerGTSAM : public Optimizer std::vector lastAddedConstraints_; int lastSwitchId_; std::set addedPoses_; + std::map isLandmarkWithRotation_; // persists across iSAM2 incremental calls std::pair lastRootFactorIndex_; }; diff --git a/corelib/src/CMakeLists.txt b/corelib/src/CMakeLists.txt index f787c40f9d..8f82257b96 100644 --- a/corelib/src/CMakeLists.txt +++ b/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} diff --git a/corelib/src/Graph.cpp b/corelib/src/Graph.cpp index 1cc96a0592..05f2ac9162 100644 --- a/corelib/src/Graph.cpp +++ b/corelib/src/Graph.cpp @@ -218,7 +218,14 @@ bool importPoses( else if(format == 4) // g2o { std::multimap 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 diff --git a/corelib/src/MarkerDetector.cpp b/corelib/src/MarkerDetector.cpp index 9ff9f69487..db3a4fd62b 100644 --- a/corelib/src/MarkerDetector.cpp +++ b/corelib/src/MarkerDetector.cpp @@ -28,17 +28,179 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include #include +#include + +#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 - parseParameters(parameters); #endif + + parseParameters(parameters); } 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 strList = uSplit(strLengths, '|'); + for(std::list::iterator iter=strList.begin(); iter!=strList.end(); ++iter) + { + std::list 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 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 MarkerDetector::detect(const cv::Mat & image, const CameraModel & model, const cv::Mat & depth, float * markerLengthOut, cv::Mat * imageWithDetections) { + UDEBUG(""); std::map detections; std::map infos = detect(image, model, depth, std::map(), imageWithDetections); @@ -144,222 +474,405 @@ std::map MarkerDetector::detect(const cv::Mat & image, const Cam std::map MarkerDetector::detect(const cv::Mat & image, const std::vector & models, const cv::Mat & depth, - const std::map & markerLengths, + const std::map & 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 allInfo; - for(size_t i=0; i subInfo = detect(subImage, model, subDepth, markerLengths, imageWithDetections?&subImageWithDetections:0); - if(ULogger::level() >= ULogger::kWarning) + if(depth.empty()) { - for(std::map::iterator iter=subInfo.begin(); iter!=subInfo.end(); ++iter) - { - std::pair::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); - } - } + UERROR("Depth image is empty, please set %s parameter to non-null.", Parameters::kMarkerLength().c_str()); + return std::map(); } - else + } + + std::vector< int > ids; + std::vector< int > cams; + std::vector< std::vector< cv::Point2f > > corners; // in stitched image + std::vector poses; // in optical frame + + // detect markers and estimate pose + if(strategy_ == kStrategyApriltag) + { +#ifdef RTABMAP_APRILTAG + cv::Mat grayImage = image; + if(image.channels() > 1) { - allInfo.insert(subInfo.begin(), subInfo.end()); + cv::cvtColor(image, grayImage, cv::COLOR_BGR2GRAY); } - if(imageWithDetections) + 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(); + } + + std::set idsAdded; + for (int i = 0; i < zarray_size(apriltagDetections); i++) { - if(i==0) + 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()) { - *imageWithDetections = cv::Mat(image.size(), subImageWithDetections.type()); + 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(!subImageWithDetections.empty()) + + if(!markerLengths_.empty() && + markerLengths_.find(det->id) == markerLengths_.end() && + extraMarkerLengths.find(det->id) == extraMarkerLengths.end()) { - subImageWithDetections.copyTo(cv::Mat(*imageWithDetections, cv::Rect(subRGBWidth*i, 0, subRGBWidth, image.rows))); + 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; } - } - } - return allInfo; -} -std::map MarkerDetector::detect(const cv::Mat & image, - const CameraModel & model, - const cv::Mat & depth, - const std::map & 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(); - } + const CameraModel & model = models[cameraIndex]; + float offsetX = cameraIndex*subRGBWidth; - std::map detections; + // Convert detection in local camera + det->c[0] -= offsetX; + for(int i=0; i<4; ++i) + { + det->p[i][0] -= offsetX; + } -#ifdef HAVE_OPENCV_ARUCO + // 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); - std::vector< int > ids; - std::vector< std::vector< cv::Point2f > > corners, rejected; - std::vector< cv::Vec3d > rvecs, tvecs; + 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(); - // 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); + // 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(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 - cv::aruco::detectMarkers(image, *dictionary_, corners, ids, *detectorParams_, rejected); + UERROR("RTAB-Map is not built with apriltag library."); #endif - UDEBUG("Markers detected=%d rejected=%d", (int)ids.size(), (int)rejected.size()); - if(ids.size() > 0) + } + else // opencv { - float rgbToDepthFactorX = 1.0f; - float rgbToDepthFactorY = 1.0f; - if(!depth.empty()) + 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 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; i0?float(model.imageWidth())/float(depth.cols):1.0f); - rgbToDepthFactorY = 1.0f/(model.imageHeight()>0?float(model.imageHeight())/float(depth.rows):1.0f); + 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 scales; - for(size_t i=0; i::const_iterator findIter = markerLengths.find(ids[i]); - if(!depth.empty() && (markerLength_ == 0 || (markerLength_<0 && findIter==markerLengths.end()))) + 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; i0 && d1>0 && d2>0 && d3>0 && d4>0) + if(tvecs[i].val[2] <=0) { - float scale = d/tvecs[i].val[2]; - - if( fabs(d-d1) < maxDepthError_ && - fabs(d-d2) < maxDepthError_ && - fabs(d-d3) < maxDepthError_ && - fabs(d-d4) < maxDepthError_) - { - length = scale; - scales.push_back(length); - tvecs[i] *= scales.back(); - UWARN("Automatic marker length estimation: id=%d depth=%fm length=%fm", ids[i], d, scales.back()); - } - else - { - UWARN("The four marker's corners should be " - "perpendicular to camera to estimate correctly " - "the marker's length. Errors: %f, %f, %f > %fm (%s). Four corners: %f %f %f %f (middle=%f). " - "Parameter %s can be set to non-null to skip automatic " - "marker length estimation. Detections are ignored.", - fabs(d1-d2), fabs(d1-d3), fabs(d1-d4), maxDepthError_, Parameters::kMarkerMaxDepthError().c_str(), - d1, d2, d3, d4, d, - Parameters::kMarkerLength().c_str()); - continue; - } + 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(0,0), R.at(0,1), R.at(0,2), tvecs[i].val[0], + R.at(1,0), R.at(1,1), R.at(1,2), tvecs[i].val[1], + R.at(2,0), R.at(2,1), R.at(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 scales; + std::map detections; + for(size_t i=0; i::const_iterator findIter = extraMarkerLengths.find(ids[i]); + 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); + float d2 = util2d::getDepth(depth, corners[i][1].x*rgbToDepthFactorX, corners[i][1].y*rgbToDepthFactorY, true, 0.02f, true); + float d3 = util2d::getDepth(depth, corners[i][2].x*rgbToDepthFactorX, corners[i][2].y*rgbToDepthFactorY, true, 0.02f, true); + float d4 = util2d::getDepth(depth, corners[i][3].x*rgbToDepthFactorX, corners[i][3].y*rgbToDepthFactorY, true, 0.02f, true); + // Accept measurement only if all 4 depth values are valid and + // they are at the same depth (camera should be perpendicular to marker for + // best depth estimation) + if(d>0 && d1>0 && d2>0 && d3>0 && d4>0) + { + float scale = d / poses[i].z(); + + if( fabs(d-d1) < maxDepthError_ && + fabs(d-d2) < maxDepthError_ && + fabs(d-d3) < maxDepthError_ && + fabs(d-d4) < maxDepthError_) + { + length = scale; + scales.push_back(length); + UWARN("Automatic marker length estimation: id=%d depth=%fm length=%fm", ids[i], d, length); } else { - UWARN("Some depth values (%f,%f,%f,%f, middle=%f) cannot be detected on the " - "marker's corners, cannot initialize marker length. " - "Parameter %s can be set to non-null to skip automatic " - "marker length estimation. Detections are ignored.", - d1,d2,d3,d4,d, - Parameters::kMarkerLength().c_str()); - continue; + UWARN("The four marker's corners should be " + "perpendicular to camera to estimate correctly " + "the marker's length. Errors: %f, %f, %f > %fm (%s). Four corners: %f %f %f %f (middle=%f). " + "Parameter %s can be set to non-null to skip automatic " + "marker length estimation. Detections are ignored.", + fabs(d1-d2), fabs(d1-d3), fabs(d1-d4), maxDepthError_, Parameters::kMarkerMaxDepthError().c_str(), + d1, d2, d3, d4, d, + Parameters::kMarkerLength().c_str()); + continue; } } - else if(markerLength_ < 0) - { - if(findIter!=markerLengths.end()) - { - 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()); - continue; - } - } - else if(markerLength_ > 0) - { - length = markerLength_; - } - else - { - continue; - } - - // 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_) && - (minRange_ <= 0 || tvecs[i].val[2] > minRange_)) + else { - cv::Mat R; - cv::Rodrigues(rvecs[i], R); - Transform t(R.at(0,0), R.at(0,1), R.at(0,2), tvecs[i].val[0], - R.at(1,0), R.at(1,1), R.at(1,2), tvecs[i].val[1], - R.at(2,0), R.at(2,1), R.at(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()); + UWARN("Some depth values (%f,%f,%f,%f, middle=%f) cannot be detected on the " + "marker's corners, cannot initialize marker length. " + "Parameter %s can be set to non-null to skip automatic " + "marker length estimation. Detections are ignored.", + d1,d2,d3,d4,d, + Parameters::kMarkerLength().c_str()); + continue; } } - if(markerLength_ == 0 && !scales.empty()) + else if(!markerLengths_.empty() || findIter!=extraMarkerLengths.end()) { - float sum = 0.0f; - float maxError = 0.0f; - for(size_t i=0; i::const_iterator paramIter = markerLengths_.find(ids[i]); + if(paramIter != markerLengths_.end()) { - if(i>0) + if(findIter!=extraMarkerLengths.end() && findIter->second != paramIter->second) { - float error = fabs(scales[i]-scales[0]); - if(error > 0.001f) - { - 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.", - scales[i], scales[0], - Parameters::kMarkerLength().c_str()); - detections.clear(); - return detections; - } - if(error > maxError) - { - maxError = error; - } + 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; + } + else + { + length = paramIter->second; } - sum += scales[i]; } - markerLength_ = sum/float(scales.size()); - UWARN("Final marker length estimated = %fm, max error=%fm (used for subsequent detections)", markerLength_, maxError); + 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_; + } + else + { + 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 || poses[i].z() < maxRange_) && + (minRange_ <= 0 || poses[i].z() > minRange_)) + { + Transform pose = models[cams[i]].localTransform() * poses[i]; + 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(), + 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()) + { + float sum = 0.0f; + float maxError = 0.0f; + for(size_t i=0; i0) + { + float error = fabs(scales[i]-scales[0]); + if(error > 0.001f) + { + 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 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(), + detections.size()); + detections.clear(); + return detections; + } + if(error > maxError) + { + maxError = error; + } + } + sum += scales[i]; + } + 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 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::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); -#else - cv::aruco::drawAxis(*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); +#elif defined(HAVE_OPENCV_ARUCO) + 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 MarkerDetector::detect(const cv::Mat & image, + const CameraModel & model, + const cv::Mat & depth, + const std::map & 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(); + } + + std::vector models; + models.push_back(model); + + return detect(image, models, depth, markerLengths, imageWithDetections); +} + } /* namespace rtabmap */ diff --git a/corelib/src/Parameters.cpp b/corelib/src/Parameters.cpp index 844f4534d8..83e1135bda 100644 --- a/corelib/src/Parameters.cpp +++ b/corelib/src/Parameters.cpp @@ -238,6 +238,9 @@ const std::map > & 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 > & 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 diff --git a/corelib/src/Rtabmap.cpp b/corelib/src/Rtabmap.cpp index af11082d6d..7a4c6f330e 100644 --- a/corelib/src/Rtabmap.cpp +++ b/corelib/src/Rtabmap.cpp @@ -4504,8 +4504,7 @@ bool Rtabmap::process( (smallDisplacement || tooFastMovement) && _loopClosureHypothesis.first == 0 && lastProximitySpaceClosureId == 0 && - !delayedLocalization && - (rejectedLoopClosure || landmarksDetected.empty())) + !delayedLocalization) { _odomCachePoses.erase(signatureRemoved); for(std::multimap::iterator iter=_odomCacheConstraints.begin(); iter!=_odomCacheConstraints.end();) diff --git a/corelib/src/optimizer/OptimizerG2O.cpp b/corelib/src/optimizer/OptimizerG2O.cpp index 4534e3463f..0eafcc24a9 100644 --- a/corelib/src/optimizer/OptimizerG2O.cpp +++ b/corelib/src/optimizer/OptimizerG2O.cpp @@ -2135,6 +2135,390 @@ std::map OptimizerG2O::optimizeBA( return optimizedPoses; } +bool OptimizerG2O::loadGraph( + const std::string & fileName, + std::map & poses, + std::multimap & 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 verticesList; + std::vector edgesList; + + char line[2048]; + while(fgets(line, 2048, file) != NULL) + { + std::list tokenList = uSplit(uReplaceChar(uReplaceChar(line, '\n', ' '), '\r', ' '), ' '); + std::vector v; + v.reserve(tokenList.size()); + for(std::list::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(0, 0) = uStr2Double(v[6]); + e.info.at(0, 1) = e.info.at(1, 0) = uStr2Double(v[7]); + e.info.at(0, 5) = e.info.at(5, 0) = uStr2Double(v[8]); + e.info.at(1, 1) = uStr2Double(v[9]); + e.info.at(1, 5) = e.info.at(5, 1) = uStr2Double(v[10]); + e.info.at(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(0, 0) = uStr2Double(v[5]); + e.info.at(0, 1) = e.info.at(1, 0) = uStr2Double(v[6]); + e.info.at(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(r, c) = uStr2Double(v[idx++]); + if(r != c) e.info.at(c, r) = e.info.at(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(0, 0) = uStr2Double(v[7]); + e.info.at(0, 1) = e.info.at(1, 0) = uStr2Double(v[8]); + e.info.at(0, 2) = e.info.at(2, 0) = uStr2Double(v[9]); + e.info.at(1, 1) = uStr2Double(v[10]); + e.info.at(1, 2) = e.info.at(2, 1) = uStr2Double(v[11]); + e.info.at(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(0, 0) = uStr2Double(v[5]); + e.info.at(0, 1) = e.info.at(1, 0) = uStr2Double(v[6]); + e.info.at(0, 5) = e.info.at(5, 0) = uStr2Double(v[7]); + e.info.at(1, 1) = uStr2Double(v[8]); + e.info.at(1, 5) = e.info.at(5, 1) = uStr2Double(v[9]); + e.info.at(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(0, 0) = uStr2Double(v[4]); + e.info.at(0, 1) = e.info.at(1, 0) = uStr2Double(v[5]); + e.info.at(1, 1) = uStr2Double(v[6]); + // no orientation info on this prior + e.info.at(3, 3) = e.info.at(4, 4) = e.info.at(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(r, c) = uStr2Double(v[idx++]); + if(r != c) e.info.at(c, r) = e.info.at(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(0, 0) = uStr2Double(v[5]); + e.info.at(0, 1) = e.info.at(1, 0) = uStr2Double(v[6]); + e.info.at(0, 2) = e.info.at(2, 0) = uStr2Double(v[7]); + e.info.at(1, 1) = uStr2Double(v[8]); + e.info.at(1, 2) = e.info.at(2, 1) = uStr2Double(v[9]); + e.info.at(2, 2) = uStr2Double(v[10]); + // no orientation info on this prior + e.info.at(3, 3) = e.info.at(4, 4) = e.info.at(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(0, 0) = uStr2Double(v[7]); + e.info.at(0, 1) = e.info.at(1, 0) = uStr2Double(v[8]); + e.info.at(0, 5) = e.info.at(5, 0) = uStr2Double(v[9]); + e.info.at(1, 1) = uStr2Double(v[10]); + e.info.at(1, 5) = e.info.at(5, 1) = uStr2Double(v[11]); + e.info.at(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(r, c) = uStr2Double(v[idx++]); + if(r != c) e.info.at(c, r) = e.info.at(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 & poses, diff --git a/corelib/src/optimizer/OptimizerGTSAM.cpp b/corelib/src/optimizer/OptimizerGTSAM.cpp index 56ac51ba28..6361f53792 100644 --- a/corelib/src/optimizer/OptimizerGTSAM.cpp +++ b/corelib/src/optimizer/OptimizerGTSAM.cpp @@ -239,6 +239,7 @@ std::map OptimizerGTSAM::optimize( isam2_ = new gtsam::ISAM2(params); addedPoses_.clear(); lastAddedConstraints_.clear(); + isLandmarkWithRotation_.clear(); lastRootFactorIndex_.first = 0; lastSwitchId_ = 1000000000; } @@ -308,7 +309,13 @@ std::map 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 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::const_iterator iter = newPoses.begin(); iter!=newPoses.end(); ++iter) { UASSERT(!iter->second.isNull()); @@ -328,12 +335,12 @@ std::map OptimizerGTSAM::optimize( if (1 / static_cast(jter->second.infMatrix().at(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 OptimizerGTSAM::optimize( 1 / static_cast(jter->second.infMatrix().at(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 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(0,0), @@ -429,7 +436,7 @@ std::map 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(0,0), @@ -501,9 +508,9 @@ std::map 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 OptimizerGTSAM::optimize( gtsam::noiseModel::Diagonal::shared_ptr switchPriorModel = gtsam::noiseModel::Diagonal::Sigmas(gtsam::Vector1(1.0)); graph.add(gtsam::PriorFactor (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 OptimizerGTSAM::optimize( if(isSlam2d()) { - if(isLandmarkWithRotation.at(id2)) + if(isLandmarkWithRotation_.at(id2)) { Eigen::Matrix information = Eigen::Matrix::Identity(); if(!isCovarianceIgnored()) @@ -592,7 +599,7 @@ std::map OptimizerGTSAM::optimize( } else { - if(isLandmarkWithRotation.at(id2)) + if(isLandmarkWithRotation_.at(id2)) { Eigen::Matrix information = Eigen::Matrix::Identity(); if(!isCovarianceIgnored()) @@ -807,9 +814,9 @@ std::map OptimizerGTSAM::optimize( gtsam::Pose2 p = iter->value.cast(); 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(); @@ -830,9 +837,9 @@ std::map OptimizerGTSAM::optimize( gtsam::Pose3 p = iter->value.cast(); 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(); tmpPoses.insert(std::make_pair(key, Transform::fromEigen4d(p.matrix()))); @@ -1000,9 +1007,9 @@ std::map OptimizerGTSAM::optimize( gtsam::Pose2 p = iter->value.cast(); 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(); @@ -1023,9 +1030,9 @@ std::map OptimizerGTSAM::optimize( gtsam::Pose3 p = iter->value.cast(); 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(); optimizedPoses.insert(std::make_pair(key, Transform::fromEigen4d(p.matrix()))); diff --git a/examples/WifiMapping/WifiThread.h b/examples/WifiMapping/WifiThread.h index 7e3d14c688..27da516f8c 100644 --- a/examples/WifiMapping/WifiThread.h +++ b/examples/WifiMapping/WifiThread.h @@ -177,7 +177,8 @@ class WifiThread : public UThread, public UEventsSender 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; diff --git a/guilib/include/rtabmap/gui/CameraViewer.h b/guilib/include/rtabmap/gui/CameraViewer.h index 906d173563..9e6a0c5bb9 100644 --- a/guilib/include/rtabmap/gui/CameraViewer.h +++ b/guilib/include/rtabmap/gui/CameraViewer.h @@ -77,6 +77,7 @@ public Q_SLOTS: QElapsedTimer fpsTimer_; double lastCapturePeriod_; double previousCaptureStamp_; + std::map _landmarksSize; }; } /* namespace rtabmap */ diff --git a/guilib/include/rtabmap/gui/PreferencesDialog.h b/guilib/include/rtabmap/gui/PreferencesDialog.h index c6be469462..65a105a2a7 100644 --- a/guilib/include/rtabmap/gui/PreferencesDialog.h +++ b/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(); diff --git a/guilib/src/AboutDialog.cpp b/guilib/src/AboutDialog.cpp index 40a605b5b1..5d13079794 100644 --- a/guilib/src/AboutDialog.cpp +++ b/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); diff --git a/guilib/src/CameraViewer.cpp b/guilib/src/CameraViewer.cpp index 601cbc6fe3..6588d51b78 100644 --- a/guilib/src/CameraViewer.cpp +++ b/guilib/src/CameraViewer.cpp @@ -36,6 +36,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include #include +#include #include #include #include @@ -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 detections; + UTimer markerDetectionTime; if(!left.empty()) { std::vector 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(), &imageWithDetections); + detections = markerDetector_->detect(left, models, depthOrRight, _landmarksSize, &imageWithDetections); imageView_->setImage(uCvMat2QImage(imageWithDetections)); + for(std::map::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, diff --git a/guilib/src/MainWindow.cpp b/guilib/src/MainWindow.cpp index 92a053f2d5..8daf26b23f 100644 --- a/guilib/src/MainWindow.cpp +++ b/guilib/src/MainWindow.cpp @@ -5239,6 +5239,7 @@ void MainWindow::drawKeypoints(const std::multimap & refWords void MainWindow::drawLandmarks(cv::Mat & image, const Signature & signature) { + UDEBUG("%ld landmarks", signature.getLandmarks().size()); for(std::map::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) diff --git a/guilib/src/PreferencesDialog.cpp b/guilib/src/PreferencesDialog.cpp index 93c55d7c5c..fd0e3cc0a7 100644 --- a/guilib/src/PreferencesDialog.cpp +++ b/guilib/src/PreferencesDialog.cpp @@ -475,15 +475,14 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) : _ui->checkBox_showOdomFrustums->setChecked(false); #endif - //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))) - _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); +#if !defined(HAVE_OPENCV_ARUCO) && !defined(RTABMAP_APRILTAG) + _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."); #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->ArucoMarkerLength->setObjectName(Parameters::kMarkerLength().c_str()); - _ui->ArucoMaxDepthError->setObjectName(Parameters::kMarkerMaxDepthError().c_str()); - _ui->ArucoVarianceLinear->setObjectName(Parameters::kMarkerVarianceLinear().c_str()); - _ui->ArucoVarianceAngular->setObjectName(Parameters::kMarkerVarianceAngular().c_str()); - _ui->ArucoVarianceOrientationIgnored->setObjectName(Parameters::kMarkerVarianceOrientationIgnored().c_str()); - _ui->ArucoMarkerRangeMin->setObjectName(Parameters::kMarkerMinRange().c_str()); - _ui->ArucoMarkerRangeMax->setObjectName(Parameters::kMarkerMaxRange().c_str()); - _ui->ArucoMarkerPriors->setObjectName(Parameters::kMarkerPriors().c_str()); - _ui->ArucoPriorsVarianceLinear->setObjectName(Parameters::kMarkerPriorsVarianceLinear().c_str()); - _ui->ArucoPriorsVarianceAngular->setObjectName(Parameters::kMarkerPriorsVarianceAngular().c_str()); - _ui->ArucoCornerRefinementMethod->setObjectName(Parameters::kMarkerCornerRefinementMethod().c_str()); + _ui->MarkerStrategy->setObjectName(Parameters::kMarkerStrategy().c_str()); + connect(_ui->MarkerStrategy, SIGNAL(currentIndexChanged(int)), _ui->stackedWidget_markerStrategy, SLOT(setCurrentIndex(int))); + connect(_ui->MarkerStrategy, SIGNAL(currentIndexChanged(int)), this, SLOT(updateAvailableMarkerDictionaries())); + _ui->MarkerStrategy->setCurrentIndex(Parameters::defaultMarkerStrategy()); + updateAvailableMarkerDictionaries(); + _ui->MarkerDictionary->setObjectName(Parameters::kMarkerDictionary().c_str()); + _ui->MarkerLength->setObjectName(Parameters::kMarkerLength().c_str()); + _ui->MarkerLengths->setObjectName(Parameters::kMarkerLengths().c_str()); + _ui->MarkerMaxDepthError->setObjectName(Parameters::kMarkerMaxDepthError().c_str()); + _ui->MarkerVarianceLinear->setObjectName(Parameters::kMarkerVarianceLinear().c_str()); + _ui->MarkerVarianceAngular->setObjectName(Parameters::kMarkerVarianceAngular().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 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->MarkerStrategy->currentIndex() == 0) { - 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.")); - _ui->ArucoDictionary->setCurrentIndex(0); +#if CV_MAJOR_VERSION < 3 || (CV_MAJOR_VERSION == 3 && (CV_MINOR_VERSION <4 || (CV_MINOR_VERSION ==4 && CV_SUBMINOR_VERSION<2))) + if(_ui->MarkerDictionary->currentIndex()>=17) + { + QMessageBox::warning(this, tr("Parameter warning"), + 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->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(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 { diff --git a/guilib/src/ui/aboutDialog.ui b/guilib/src/ui/aboutDialog.ui index ca0e087d1e..532c976461 100644 --- a/guilib/src/ui/aboutDialog.ui +++ b/guilib/src/ui/aboutDialog.ui @@ -164,7 +164,7 @@ p, li { white-space: pre-wrap; } 0 0 596 - 1257 + 1280 @@ -185,41 +185,28 @@ p, li { white-space: pre-wrap; } - - - - - - - Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter - - - true - - - - - + + - BSD + Apache v2 true - - + + - BSD + With CPU-TSDF : true - - + + @@ -231,87 +218,71 @@ p, li { white-space: pre-wrap; } - - - - GPLv2 - - - true - - - - - + + - - - - Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + With CudaSift : true - - + + - - - - Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + With Zed SDK : true - - + + - + <html><head/><body><p><span style=" font-weight:600;">Third Party Libraries</span></p></body></html> - - Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + + Qt::RichText true - - + + - MIT + With PDAL : true - - + + - With cvsba : + BSD true - - + + - MIT + With Zed Open Capture : true - - + + BSD @@ -320,58 +291,58 @@ p, li { white-space: pre-wrap; } - - + + - BSD + With ORB SLAM 2 : true - - + + - With Viso2 : + With MSCKF : true - - + + - With libLAS : + GPLv3 true - - + + - With K4A : + BSD true - - + + - Creative Commons [Attribution-NonCommercial-ShareAlike] + With K4W2 : true - - + + @@ -383,67 +354,76 @@ p, li { white-space: pre-wrap; } - - + + - With CCCoreLib : + With OpenChisel : true - - + + - With RealSense2 : + MIT true - - + + - With stereo dc1394 : + With RealSense : true - - + + - With OpenChisel : + + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter true - - + + - MIT + + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter true - - + + - GPLv3 + + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter true - + @@ -456,28 +436,28 @@ p, li { white-space: pre-wrap; } - - + + - With g2o : + With Freenect2 : true - - + + - BSD + <html><head/><body><p><span style=" font-weight:600;">License</span></p></body></html> true - - + + @@ -489,38 +469,38 @@ p, li { white-space: pre-wrap; } - - + + - BSD + GPLv3 true - - + + - With FastCV : + BSD true - - + + - BSD + With libpointmatcher : true - - + + @@ -532,68 +512,71 @@ p, li { white-space: pre-wrap; } - - + + - MPL2 + BSD true - - + + - MIT + With MYNTEYE S : true - - + + - With stereo FlyCapture2 : + BSD true - - + + - GPLv3 + Creative Commons [Attribution-NonCommercial-ShareAlike] true - - + + - MIT + + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter true - - + + - With VINS-Fusion : + BSD true - - + + @@ -605,128 +588,133 @@ p, li { white-space: pre-wrap; } - - + + - BSD + GPLv3 true - - + + - With Zed Open Capture : + GPLv3 true - - + + - With ORB SLAM 2 : - - - true + - - - - - - BSD + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter true - - + + - BSD + + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter true - - + + - With MSCKF : + + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter true - - + + - With CPU-TSDF : + BSD true - - + + - With Zed SDK : + + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter true - - + + - With CudaSift : + With RealSense2 : true - - + + - Apache v2 + With g2o : true - - + + - With FOVIS : + + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter true - - + + - PSF + MIT true - - + + @@ -738,28 +726,18 @@ p, li { white-space: pre-wrap; } - - - - BSD - - - true - - - - - + + - Apache v2 and/or GPLv2 + Penn Software License true - - + + @@ -771,48 +749,54 @@ p, li { white-space: pre-wrap; } - - + + - BSD + With Orbbec SDK : true - - + + - With OKVIS : + + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter true - - + + - BSD + + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter true - - + + - With libpointmatcher : + With OpenGV : true - - + + @@ -824,8 +808,8 @@ p, li { white-space: pre-wrap; } - - + + @@ -837,8 +821,8 @@ p, li { white-space: pre-wrap; } - - + + @@ -850,107 +834,101 @@ p, li { white-space: pre-wrap; } - - + + - With Octomap : + With XVisio SDK : true - - + + - GPLv2 + + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter true - - + + - - - - Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + With loam_velodyne : true - - + + - With Ceres : + With TORO : true - - + + - LGPL + With OpenNI : true - - + + - - - - Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + Apache v2 true - - + + - - - - Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + With cuVSLAM : true - - + + - With MYNTEYE S : + With Python3 : true - - + + - GPLv2 + With Viso2 : true - - + + @@ -962,28 +940,28 @@ p, li { white-space: pre-wrap; } - - + + - BSD + With FOVIS : true - - + + - With OpenNI2 : + PCL version : true - - + + @@ -995,86 +973,74 @@ p, li { white-space: pre-wrap; } - - + + - With XVisio SDK : + With cvsba : true - - + + - - - - Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter - - - true + With SuperPoint Rpautrat : - - + + - - - - Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter - - - true + MIT - - + + - - - - Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + With OpenNI2 : true - - + + - Apache-2 + BSD true - - + + - GPLv3 + + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter true - - + + - NVIDIA ISAAC ROS SOFTWARE LICENSE + MPL2 true - + @@ -1087,28 +1053,38 @@ p, li { white-space: pre-wrap; } - - + + - Apache-2 + BSD true - - + + - With Freenect2 : + With AliceVision : true - - + + + + BSD + + + true + + + + + @@ -1120,30 +1096,27 @@ p, li { white-space: pre-wrap; } - - + + - With PDAL : + MIT true - - + + - - - - Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + BSD true - + MIT @@ -1153,8 +1126,8 @@ p, li { white-space: pre-wrap; } - - + + @@ -1166,38 +1139,44 @@ p, li { white-space: pre-wrap; } - - + + - With Python3 : + + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter true - - + + - With ORB Octree : + + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter true - - + + - With DVO : + With FastCV : true - - + + @@ -1209,21 +1188,28 @@ p, li { white-space: pre-wrap; } - - + + - + With Open3D : - - Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + + true + + + + + + + Apache-2 true - - + + @@ -1235,21 +1221,28 @@ p, li { white-space: pre-wrap; } - - + + - + BSD - - Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + + true + + + + + + + With VINS-Fusion : true - - + + @@ -1261,21 +1254,18 @@ p, li { white-space: pre-wrap; } - - + + - - - - Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + GPLv2 true - - + + @@ -1287,28 +1277,28 @@ p, li { white-space: pre-wrap; } - - + + - With GridMap : + BSD true - - + + - With cuVSLAM : + With OKVIS : true - - + + @@ -1320,28 +1310,21 @@ p, li { white-space: pre-wrap; } - - + + - Penn Software License - - - true + - - - - - - Open Source or Commercial + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter true - - + + @@ -1353,8 +1336,8 @@ p, li { white-space: pre-wrap; } - - + + @@ -1366,18 +1349,18 @@ p, li { white-space: pre-wrap; } - - + + - With OpenVINS : + With stereo dc1394 : true - - + + GPLv3 @@ -1386,35 +1369,38 @@ p, li { white-space: pre-wrap; } - - + + - GPLv3 + Apache v2 and/or GPLv2 true - - + + - With Open3D : + With OpenVINS : true - - + + - + Apache v2 and/or GPLv2 + + + true - - + + @@ -1426,64 +1412,78 @@ p, li { white-space: pre-wrap; } - - + + - + Qt version : - - Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + + true + + + + + + + GPLv2 true - - + + - With TORO : + OpenCV version : true - - + + - + NVIDIA ISAAC ROS SOFTWARE LICENSE - - Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + + true + + + + + + + Apache-2 true - - + + - With GTSAM : + GPLv3 true - - + + - Apache v2 + MIT true - - + + @@ -1495,28 +1495,28 @@ p, li { white-space: pre-wrap; } - - + + - Apache v2 + BSD true - - + + - With OpenNI : + BSD true - - + + @@ -1528,38 +1528,38 @@ p, li { white-space: pre-wrap; } - - + + - With Orbbec SDK : + Open Source or Commercial true - - + + - With Freenect : + With Ceres : true - - + + - With RealSense : + LGPL true - - + + @@ -1571,31 +1571,41 @@ p, li { white-space: pre-wrap; } - - + + - With DepthAI : + + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter true - - + + - + GPLv2 - - Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + + true + + + + + + + With K4A : true - - + + @@ -1607,52 +1617,54 @@ p, li { white-space: pre-wrap; } - - + + - <html><head/><body><p><span style=" font-weight:600;">License</span></p></body></html> + + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter true - - + + - With AliceVision : + With DepthAI : true - - + + - With MRPT : + + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter true - - + + - MIT + With Freenect : - - - - - - With SuperPoint Rpautrat : + + true - - + + @@ -1664,18 +1676,18 @@ p, li { white-space: pre-wrap; } - - + + - PCL version : + MIT true - - + + @@ -1687,51 +1699,55 @@ p, li { white-space: pre-wrap; } - - + + - <html><head/><body><p><span style=" font-weight:600;">Third Party Libraries</span></p></body></html> - - - Qt::RichText + With MRPT : true - - + + - BSD + With SuperPoint Torch : true - - + + - GPLv3 + PSF true - - + + - GPLv3 + With CCCoreLib : true - - + + + + + + + + + @@ -1743,8 +1759,18 @@ p, li { white-space: pre-wrap; } - - + + + + With Octomap : + + + true + + + + + @@ -1756,71 +1782,78 @@ p, li { white-space: pre-wrap; } - - + + - With loam_velodyne : + With DVO : true - - + + - Qt version : + Apache v2 true - - + + - + With GridMap : - - Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + + true + + + + + + + With libLAS : true - - + + - Apache v2 and/or GPLv2 + VTK version : true - - + + - With SuperPoint Torch : + With GTSAM : true - - + + - VTK version : + With ORB Octree : true - - + + @@ -1832,48 +1865,48 @@ p, li { white-space: pre-wrap; } - - + + - With K4W2 : + GPLv3 true - - + + - OpenCV version : + With stereo FlyCapture2 : true - - + + - With OpenGV : + BSD true - - + + - BSD + With AprilTag : true - - + + diff --git a/guilib/src/ui/preferencesDialog.ui b/guilib/src/ui/preferencesDialog.ui index 4d11c20ef7..d03332e4f4 100644 --- a/guilib/src/ui/preferencesDialog.ui +++ b/guilib/src/ui/preferencesDialog.ui @@ -63,7 +63,7 @@ 0 - -700 + 0 684 5218 @@ -95,7 +95,7 @@ QFrame::Raised - 8 + 18 @@ -15505,7 +15505,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - + @@ -15514,30 +15514,29 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - - - m - - - 2 - - - 0.000000000000000 - - - 999.000000000000000 - - - 1.000000000000000 - - - 0.000000000000000 + + + + + opencv-aruco + + + + + apriltag + + + + + + + + - - + + @@ -15547,9 +15546,6 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag 0.000001000000000 - - 9999.000000000000000 - 0.001000000000000 @@ -15558,8 +15554,8 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - + + m @@ -15577,23 +15573,10 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - + - 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 ("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). - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - Minimum detection range (0=disabled). + 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 ("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). true @@ -15604,31 +15587,31 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - + - + m - 6 + 2 - 0.000001000000000 + 0.000000000000000 - 9999.000000000000000 + 999.000000000000000 - 0.001000000000000 + 1.000000000000000 - 0.001000000000000 + 0.000000000000000 - - + + - Linear variance to set on marker priors. + 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). true @@ -15638,10 +15621,14 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - + + + + 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. + - Angular variance to set on marker priors. + Marker length. The length (m) of the markers' side. Value &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. true @@ -15651,15 +15638,21 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - + + - + Linear variance to set on marker priors. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - + + @@ -15669,6 +15662,9 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag 0.000001000000000 + + 9999.000000000000000 + 0.001000000000000 @@ -15678,50 +15674,123 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - - m - - - 4 - - - 0.000100000000000 - - - 0.010000000000000 - - - 0.100000000000000 - - - - - - - m - - - 2 - - - 0.000000000000000 - - - 999.000000000000000 - - - 1.000000000000000 - - - 0.000000000000000 - + + + + ARUCO_4X4_50 + + + + + ARUCO_4X4_100 + + + + + ARUCO_4X4_250 + + + + + ARUCO_4X4_1000 + + + + + ARUCO_5X5_50 + + + + + ARUCO_5X5_100 + + + + + ARUCO_5X5_250 + + + + + ARUCO_5X5_1000 + + + + + ARUCO_6X6_50 + + + + + ARUCO_6X6_100 + + + + + ARUCO_6X6_250 + + + + + ARUCO_6X6_1000 + + + + + ARUCO_7X7_50 + + + + + ARUCO_7X7_100 + + + + + ARUCO_7X7_250 + + + + + ARUCO_7X7_1000 + + + + + ARUCO_ORIGINAL + + + + + APRILTAG_16h5 + + + + + APRILTAG_25h9 + + + + + APRILTAG_36h10 + + + + + APRILTAG_36h11 + + + + + ARUCO_MIP_36h12 + + - + - 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. + Dictionary to use. true @@ -15731,10 +15800,10 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - + + - 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. + Detect static markers to be added as landmarks for graph optimization. If input data have already landmarks, this will be ignored. true @@ -15744,10 +15813,10 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - + + - 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). + 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. true @@ -15757,29 +15826,17 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - - + + + - - 6 - - - 0.000001000000000 - - - 0.001000000000000 - - - 0.001000000000000 - - - + + - Maximum detection range (0=unlimited). + Angular variance to set on marker priors. true @@ -15789,17 +15846,48 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - - - + + + + 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). + + + Adjust variance to ignore orientation during optimization. Mouseover for more details. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - + + + + m + + + 2 + + + 0.000000000000000 + + + 999.000000000000000 + + + 1.000000000000000 + + + 0.000000000000000 + + + + + - 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). + Minimum detection range (0=disabled). true @@ -15809,10 +15897,10 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - + + - Detect static markers to be added as landmarks for graph optimization. If input data have already landmarks, this will be ignored. + Maximum detection range (0=unlimited). true @@ -15823,12 +15911,9 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - - 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). - + - Adjust variance to ignore orientation during optimization. Mouseover for more details. + 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. true @@ -15839,180 +15924,347 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - + + + m + + + 4 + + + 0.000100000000000 + + + 0.010000000000000 + + + 0.100000000000000 + + + + + + + + + + + + + + + + + 6 + + + 0.000001000000000 + + + 0.001000000000000 + + + 0.001000000000000 + + + + + + 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. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + + + 6 + + + 0.000001000000000 + + + 9999.000000000000000 + + + 0.001000000000000 + + + 0.001000000000000 + + + + + + + + + + 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" + + + List of markers to detect. Format is the marker's ID followed by its length (in meters), 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 Marker Length. Mouse over this label for example. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + - - - ArUco + + + 0 - - - - - - 4X4_50 - - - - - 4X4_100 - - - - - 4X4_250 - - - - - 4X4_1000 - - - - - 5X5_50 - - - - - 5X5_100 - - - - - 5X5_250 - - - - - 5X5_1000 - - - - - 6X6_50 - - - - - 6X6_100 - - - - - 6X6_250 - - - - - 6X6_1000 - - - - - 7X7_50 - - - - - 7X7_100 - - - - - 7X7_250 - - - - - 7X7_1000 - - - - - ARUCO_ORIGINAL - - - - - APRILTAG_16h5 - - - - - APRILTAG_25h9 - - - - - APRILTAG_36h10 - - - - - APRILTAG_36h11 - - - - - - - - Dictionary to use. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - - None - - - - - Subpixel - - - - - Contour + + + + + + OpenCV - - - - AprilTag 2 + + + + + Corner refinement method. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + None + + + + + Subpixel + + + + + Contour + + + + + AprilTag 2 + + + + + + + + Qt::Vertical + + + + 0 + 0 + + + + + + + + + + + + + + + AprilTag - - - - - - - Corner refinement method. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - + + + + + 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. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + nthreads: How many threads should be used? + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + 1 + + + 1 + + + + + + + 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. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + refine_edges: 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 quad_decimate = 1. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + 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). + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + 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). + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + + + + + + + + + + + + + + + + + 2 + + + 0.000000000000000 + + + 0.050000000000000 + + + 0.250000000000000 + + + + + + + + + + 1 + + + 0.000000000000000 + + + 99.900000000000006 + + + 0.100000000000000 + + + 0.000000000000000 + + + + + + + 1 + + + 1.000000000000000 + + + 2.000000000000000 + + + + + + + + diff --git a/tools/Export/main.cpp b/tools/Export/main.cpp index 18c79d228a..572e8453d6 100644 --- a/tools/Export/main.cpp +++ b/tools/Export/main.cpp @@ -1978,11 +1978,12 @@ int main(int argc, char * argv[]) exportPosesFormat, std::map(robotPoses.lower_bound(1), robotPoses.end()), links, - std::map(robotStamps.lower_bound(1), robotStamps.end())); + std::map(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(), cameraStamps[i]); + rtabmap::graph::exportPoses(outputPath, exportPosesFormat, cameraPoses[i], std::multimap(), 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(), scanStamps); + rtabmap::graph::exportPoses(outputPath, exportPosesFormat, scanPoses, std::multimap(), 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(), scanStamps); + rtabmap::graph::exportPoses(outputPath, exportPosesFormat, scanPoses, std::multimap(), 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(), landmarkStamps); + rtabmap::graph::exportPoses(outputPath, exportPosesFormat, landmarkPoses, std::multimap(), 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(), gpsStamps); + rtabmap::graph::exportPoses(outputPath, exportPosesFormat, gpsPoses, std::multimap(), 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(), gtStamps); + rtabmap::graph::exportPoses(outputPath, exportPosesFormat, gtPoses, std::multimap(), gtStamps, parameters); printf("%d scan poses exported to \"%s\".\n", (int)gtPoses.size(), outputPath.c_str());