From 19515f0c654cc539301fe4dc05022bca4235ec47 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Tue, 12 May 2026 17:05:40 -0700 Subject: [PATCH 01/19] Adding support for AprilTag v3 library --- CMakeLists.txt | 19 ++ Version.h.in | 1 + corelib/include/rtabmap/core/MarkerDetector.h | 15 +- corelib/include/rtabmap/core/Parameters.h | 1 + corelib/src/CMakeLists.txt | 7 + corelib/src/MarkerDetector.cpp | 205 ++++++++++++++---- 6 files changed, 207 insertions(+), 41 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index f80448f538..53eb538a0b 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -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,13 @@ IF(WITH_FASTCV) ENDIF(FastCV_FOUND) ENDIF(WITH_FASTCV) +IF(WITH_APRILTAG) + FIND_PACKAGE(apriltag QUIET) + IF(apriltag_FOUND) + MESSAGE(STATUS "Found apriltag") + ENDIF(apriltag_FOUND) +ENDIF(WITH_APRILTAG) + IF(WITH_OPENGV OR okvis_FOUND) if(NOT BUILD_OPENGV) FIND_PACKAGE(opengv QUIET) @@ -1066,6 +1074,9 @@ ENDIF(NOT Open3D_FOUND) IF(NOT FastCV_FOUND) SET(FASTCV "//") ENDIF(NOT FastCV_FOUND) +IF(NOT apriltag_FOUND) + SET(APRILTAG "//") +ENDIF(NOT apriltag_FOUND) IF(NOT opengv_FOUND OR NOT WITH_OPENGV) SET(OPENGV "//") ENDIF(NOT opengv_FOUND OR NOT WITH_OPENGV) @@ -1571,6 +1582,14 @@ ELSE() MESSAGE(STATUS " With FastCV = NO (FastCV not found)") ENDIF() +IF(apriltag_FOUND) +MESSAGE(STATUS " With AprilTag ${apriltag_VERSION} = YES (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..714761a727 100644 --- a/Version.h.in +++ b/Version.h.in @@ -93,6 +93,7 @@ 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 #include diff --git a/corelib/include/rtabmap/core/MarkerDetector.h b/corelib/include/rtabmap/core/MarkerDetector.h index d628ca022d..75e53b5398 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,18 @@ class RTABMAP_CORE_EXPORT MarkerDetector { cv::Mat * imageWithDetections = 0); private: -#ifdef HAVE_OPENCV_ARUCO - cv::Ptr detectorParams_; - float markerLength_; + Strategy strategy_; + float markerLength_; 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 5d3e315d2a..899286cd6e 100644 --- a/corelib/include/rtabmap/core/Parameters.h +++ b/corelib/include/rtabmap/core/Parameters.h @@ -917,6 +917,7 @@ 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, 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"); 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, 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())); diff --git a/corelib/src/CMakeLists.txt b/corelib/src/CMakeLists.txt index f787c40f9d..253499d67f 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 + ${LIBRARIES} + apriltag::apriltag + ) +ENDIF(apriltag_FOUND) + IF(opengv_FOUND) SET(LIBRARIES ${LIBRARIES} diff --git a/corelib/src/MarkerDetector.cpp b/corelib/src/MarkerDetector.cpp index 9ff9f69487..31cbf50b58 100644 --- a/corelib/src/MarkerDetector.cpp +++ b/corelib/src/MarkerDetector.cpp @@ -29,16 +29,27 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include +#ifdef RTABMAP_APRILTAG +extern "C" { +#include "apriltag/apriltag.h" +#include "apriltag/apriltag_pose.h" +#include "apriltag/tag36h11.h" +} +#endif + namespace rtabmap { -MarkerDetector::MarkerDetector(const ParametersMap & parameters) +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) @@ -53,16 +64,35 @@ MarkerDetector::MarkerDetector(const ParametersMap & parameters) #else detectorParams_->doCornerRefinement = Parameters::defaultMarkerCornerRefinementMethod()!=0; #endif - parseParameters(parameters); #endif + + parseParameters(parameters); } MarkerDetector::~MarkerDetector() { - +#ifdef RTABMAP_APRILTAG + if(apriltagLibDetector_) + { + apriltag_detector_destroy(((apriltag_detector_t*)apriltagLibDetector_)); + } + if(apriltagLibFamily_) + { + tag36h11_destroy((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_); + #ifdef HAVE_OPENCV_ARUCO detectorParams_->adaptiveThreshWinSizeMin = 3; detectorParams_->adaptiveThreshWinSizeMax = 23; @@ -95,13 +125,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)", @@ -121,6 +147,28 @@ void MarkerDetector::parseParameters(const ParametersMap & parameters) *dictionary_ = cv::aruco::getPredefinedDictionary(cv::aruco::PREDEFINED_DICTIONARY_NAME(dictionaryId_)); #endif #endif + +#ifdef RTABMAP_APRILTAG + if(apriltagLibDetector_) + { + apriltag_detector_destroy(((apriltag_detector_t*)apriltagLibDetector_)); + apriltagLibDetector_ = NULL; + } + if(apriltagLibFamily_) + { + tag36h11_destroy((apriltag_family_t*)apriltagLibFamily_); + apriltagLibFamily_ = NULL; + } + apriltagLibDetector_ = apriltag_detector_create(); + ((apriltag_detector_t*)apriltagLibDetector_)->nthreads = 2; + ((apriltag_detector_t*)apriltagLibDetector_)->quad_decimate = 1; + apriltagLibFamily_ = tag36h11_create(); + 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."); + } +#endif } std::map MarkerDetector::detect(const cv::Mat & image, const CameraModel & model, const cv::Mat & depth, float * markerLengthOut, cv::Mat * imageWithDetections) @@ -207,18 +255,97 @@ std::map MarkerDetector::detect(const cv::Mat & image, std::map detections; -#ifdef HAVE_OPENCV_ARUCO - std::vector< int > ids; std::vector< std::vector< cv::Point2f > > corners, rejected; - std::vector< cv::Vec3d > rvecs, tvecs; + std::vector poses; // detect markers and estimate pose + if(strategy_ == kStrategyApriltag) + { +#ifdef RTABMAP_APRILTAG + // Make an image_u8_t header for the Mat data + UASSERT(image.type() == CV_8UC1); + image_u8_t im = {image.cols, image.rows, (int)image.step, image.data}; + + zarray_t *apriltagDetections = apriltag_detector_detect(((apriltag_detector_t*)apriltagLibDetector_), &im); + + if (errno == EAGAIN) { + UFATAL("Unable to create the %d threads requested.", ((apriltag_detector_t*)apriltagLibDetector_)->nthreads); + exit(-1); + } + + for (int i = 0; i < zarray_size(apriltagDetections); i++) { + apriltag_detection_t *det; + zarray_get(apriltagDetections, i, &det); + + apriltag_detection_info_t info; + info.det = det; + info.tagsize = markerLength_<=0.0?1.0f:markerLength_; + info.fx = model.fx(); + info.fy = model.fy(); + info.cx = model.cx(); + info.cy = model.cy(); + + // Then call estimate_tag_pose. + apriltag_pose_t pose; + double err = estimate_tag_pose(&info, &pose); + if (pose.R && pose.t) + { + 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); + + corners.push_back(std::vector(4)); + for(int i=0; i<4; ++i) + { + corners.back()[i].x = det->p[i][0]; + corners.back()[i].y = det->p[i][1]; + UDEBUG("Marker %d corner %d : %f %f", det->id, i, corners.back()[i].x, corners.back()[i].y); + } + ids.push_back(det->id); + UDEBUG("Add marker %d (err = %f)", det->id, err); + } + else + { + UWARN("Failed to compute pose for marker %d, ignoring...", det->id); + } + + // Free pose memory + if (pose.R) matd_destroy(pose.R); + if (pose.t) matd_destroy(pose.t); + } + + apriltag_detections_destroy(apriltagDetections); +#else + UERROR("RTAB-Map is not built with apriltag library."); +#endif + } + else // opencv + { + std::vector< cv::Vec3d > rvecs, tvecs; +#ifdef HAVE_OPENCV_ARUCO #if CV_MAJOR_VERSION > 3 || (CV_MAJOR_VERSION == 3 && CV_MINOR_VERSION >=2) - cv::aruco::detectMarkers(image, dictionary_, corners, ids, detectorParams_, rejected); + cv::aruco::detectMarkers(image, dictionary_, corners, ids, detectorParams_, rejected); #else - cv::aruco::detectMarkers(image, *dictionary_, corners, ids, *detectorParams_, rejected); + cv::aruco::detectMarkers(image, *dictionary_, corners, ids, *detectorParams_, rejected); +#endif + cv::aruco::estimatePoseSingleMarkers(corners, markerLength_<=0.0?1.0f:markerLength_, model.K(), model.D(), rvecs, tvecs); + + for(size_t i=0; i(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); + } +#else + UERROR("RTAB-Map is not built with \"aruco\" module from OpenCV."); #endif + } + UDEBUG("Markers detected=%d rejected=%d", (int)ids.size(), (int)rejected.size()); if(ids.size() > 0) { @@ -238,7 +365,6 @@ std::map MarkerDetector::detect(const cv::Mat & image, } } - cv::aruco::estimatePoseSingleMarkers(corners, markerLength_<=0.0?1.0f:markerLength_, model.K(), model.D(), rvecs, tvecs); std::vector scales; for(size_t i=0; i MarkerDetector::detect(const cv::Mat & image, // best depth estimation) if(d>0 && d1>0 && d2>0 && d3>0 && d4>0) { - float scale = d/tvecs[i].val[2]; + float scale = d / poses[i].z(); if( fabs(d-d1) < maxDepthError_ && fabs(d-d2) < maxDepthError_ && @@ -265,8 +391,10 @@ std::map MarkerDetector::detect(const cv::Mat & image, { length = scale; scales.push_back(length); - tvecs[i] *= scales.back(); - UWARN("Automatic marker length estimation: id=%d depth=%fm length=%fm", ids[i], d, scales.back()); + poses[i].x() *= length; + poses[i].y() *= length; + poses[i].z() *= length; + UWARN("Automatic marker length estimation: id=%d depth=%fm length=%fm", ids[i], d, length); } else { @@ -297,7 +425,9 @@ std::map MarkerDetector::detect(const cv::Mat & image, if(findIter!=markerLengths.end()) { length = findIter->second; - tvecs[i] *= length; + poses[i].x() *= length; + poses[i].y() *= length; + poses[i].z() *= length; } else { @@ -316,17 +446,13 @@ std::map MarkerDetector::detect(const cv::Mat & image, // 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_)) + if((maxRange_ <= 0 || poses[i].z() < maxRange_) && + (minRange_ <= 0 || poses[i].z() > minRange_)) { - 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; + Transform pose = model.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(), t.prettyPrint().c_str(), model.localTransform().prettyPrint().c_str()); + UDEBUG("Marker %d detected in base_link: %s, optical_link=%s, local transform=%s", + ids[i], pose.prettyPrint().c_str(), poses[i].prettyPrint().c_str(), model.localTransform().prettyPrint().c_str()); } } if(markerLength_ == 0 && !scales.empty()) @@ -363,6 +489,7 @@ std::map MarkerDetector::detect(const cv::Mat & image, if(imageWithDetections) { +#ifdef HAVE_OPENCV_ARUCO if(image.channels()==1) { cv::cvtColor(image, *imageWithDetections, cv::COLOR_GRAY2BGR); @@ -380,19 +507,21 @@ std::map MarkerDetector::detect(const cv::Mat & image, std::map::iterator iter = detections.find(ids[i]); if(iter!=detections.end()) { + cv::Vec3d rvec; + cv::Vec3d tvec(poses[i].x(), poses[i].y(), poses[i].z()); + cv::Rodrigues(poses[i].rotationMatrix(), rvec); #if CV_MAJOR_VERSION > 4 || (CV_MAJOR_VERSION == 4 && (CV_MINOR_VERSION >1 || (CV_MINOR_VERSION==1 && CV_PATCH_VERSION>=1))) - cv::drawFrameAxes(*imageWithDetections, model.K(), model.D(), rvecs[i], tvecs[i], iter->second.length() * 0.5f); + cv::drawFrameAxes(*imageWithDetections, model.K(), model.D(), rvec, tvec, iter->second.length() * 0.5f); #else - cv::aruco::drawAxis(*imageWithDetections, model.K(), model.D(), rvecs[i], tvecs[i], iter->second.length() * 0.5f); + cv::aruco::drawAxis(*imageWithDetections, model.K(), model.D(), rvec, tvec, iter->second.length() * 0.5f); #endif } } } - } - #else - UERROR("RTAB-Map is not built with \"aruco\" module from OpenCV."); + UERROR("RTAB-Map is not built with \"aruco\" module from OpenCV. Cannot draw markers on image."); #endif + } return detections; } From 09c8bc80c1cceb9a85133ca4184983296e9bed5c Mon Sep 17 00:00:00 2001 From: matlabbe Date: Thu, 14 May 2026 14:12:23 -0700 Subject: [PATCH 02/19] New parameter: Marker/Strategy (default opencv-aruco as before). Optimized multicameras marker detection (do only once with stitched image) --- corelib/include/rtabmap/core/Parameters.h | 2 +- corelib/src/MarkerDetector.cpp | 441 +++++++++-------- guilib/src/PreferencesDialog.cpp | 3 +- guilib/src/ui/preferencesDialog.ui | 566 ++++++++++++---------- 4 files changed, 558 insertions(+), 454 deletions(-) diff --git a/corelib/include/rtabmap/core/Parameters.h b/corelib/include/rtabmap/core/Parameters.h index 899286cd6e..3e1dddc06d 100644 --- a/corelib/include/rtabmap/core/Parameters.h +++ b/corelib/include/rtabmap/core/Parameters.h @@ -919,7 +919,7 @@ class RTABMAP_CORE_EXPORT Parameters 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"); - 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, 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(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())); diff --git a/corelib/src/MarkerDetector.cpp b/corelib/src/MarkerDetector.cpp index 31cbf50b58..2a8f890f5d 100644 --- a/corelib/src/MarkerDetector.cpp +++ b/corelib/src/MarkerDetector.cpp @@ -171,6 +171,7 @@ void MarkerDetector::parseParameters(const ParametersMap & parameters) #endif } +// deprecated std::map MarkerDetector::detect(const cv::Mat & image, const CameraModel & model, const cv::Mat & depth, float * markerLengthOut, cv::Mat * imageWithDetections) { std::map detections; @@ -199,88 +200,77 @@ std::map MarkerDetector::detect(const cv::Mat & image, 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) - { - 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); - } - } - } - else - { - allInfo.insert(subInfo.begin(), subInfo.end()); - } - if(imageWithDetections) - { - if(i==0) - { - *imageWithDetections = cv::Mat(image.size(), subImageWithDetections.type()); - } - if(!subImageWithDetections.empty()) - { - subImageWithDetections.copyTo(cv::Mat(*imageWithDetections, cv::Rect(subRGBWidth*i, 0, subRGBWidth, image.rows))); - } - } + rgbToDepthFactorX = float(depth.cols) / float(image.cols); + rgbToDepthFactorY = float(depth.rows) / float(image.rows); } - 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()) + else if(markerLength_ == 0) { - UERROR("This method cannot handle multi-camera marker detection, use the other function version supporting it."); - return std::map(); + if(depth.empty()) + { + UERROR("Depth image is empty, please set %s parameter to non-null.", Parameters::kMarkerLength().c_str()); + return std::map(); + } } - std::map detections; - std::vector< int > ids; - std::vector< std::vector< cv::Point2f > > corners, rejected; + std::vector< int > cams; + std::vector< std::vector< cv::Point2f > > corners; // in stitched image std::vector poses; // detect markers and estimate pose if(strategy_ == kStrategyApriltag) { #ifdef RTABMAP_APRILTAG - // Make an image_u8_t header for the Mat data UASSERT(image.type() == CV_8UC1); image_u8_t im = {image.cols, image.rows, (int)image.step, image.data}; zarray_t *apriltagDetections = apriltag_detector_detect(((apriltag_detector_t*)apriltagLibDetector_), &im); if (errno == EAGAIN) { - UFATAL("Unable to create the %d threads requested.", ((apriltag_detector_t*)apriltagLibDetector_)->nthreads); - exit(-1); + UERROR("Unable to create the %d threads requested.", ((apriltag_detector_t*)apriltagLibDetector_)->nthreads); + if(apriltagDetections) + { + apriltag_detections_destroy(apriltagDetections); + } + return std::map(); } - for (int i = 0; i < zarray_size(apriltagDetections); i++) { + std::set idsAdded; + for (int i = 0; i < zarray_size(apriltagDetections); i++) + { apriltag_detection_t *det; zarray_get(apriltagDetections, i, &det); + // Which camera? + int cameraIndex = int(det->c[0]) / subRGBWidth; + UASSERT(cameraIndex>=0 && cameraIndex<(int)models.size()); + + if(idsAdded.find(det->id)!=idsAdded.end()) + { + UWARN("Marker %d already added by another camera, ignoring detection from camera %d", det->id, cameraIndex); + continue; + } + + const CameraModel & model = models[cameraIndex]; + float offsetX = cameraIndex*subRGBWidth; + + // Convert detection in local camera + det->c[0] -= offsetX; + for(int i=0; i<4; ++i) + { + det->p[i][0] -= offsetX; + } + + // FIXME, homography needs to be updated + apriltag_detection_info_t info; info.det = det; - info.tagsize = markerLength_<=0.0?1.0f:markerLength_; + info.tagsize = 1.0f; info.fx = model.fx(); info.fy = model.fy(); info.cx = model.cx(); @@ -291,20 +281,22 @@ std::map MarkerDetector::detect(const cv::Mat & image, double err = estimate_tag_pose(&info, &pose); if (pose.R && pose.t) { - 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)); + Transform t(MATD_EL(pose.R, 0, 0), MATD_EL(pose.R, 1, 0), MATD_EL(pose.R, 2, 0), MATD_EL(pose.t, 0, 0), + MATD_EL(pose.R, 0, 1), MATD_EL(pose.R, 1, 1), MATD_EL(pose.R, 2, 1), MATD_EL(pose.t, 1, 0), + MATD_EL(pose.R, 0, 2), MATD_EL(pose.R, 1, 2), MATD_EL(pose.R, 2, 2), MATD_EL(pose.t, 2, 0)); poses.push_back(t); corners.push_back(std::vector(4)); for(int i=0; i<4; ++i) { - corners.back()[i].x = det->p[i][0]; + // reconvert in original stitched image + corners.back()[i].x = det->p[i][0]+offsetX; corners.back()[i].y = det->p[i][1]; - UDEBUG("Marker %d corner %d : %f %f", det->id, i, corners.back()[i].x, corners.back()[i].y); } ids.push_back(det->id); + cams.push_back(cameraIndex); UDEBUG("Add marker %d (err = %f)", det->id, err); + idsAdded.insert(det->id); } else { @@ -323,168 +315,206 @@ std::map MarkerDetector::detect(const cv::Mat & image, } else // opencv { - std::vector< cv::Vec3d > rvecs, tvecs; + 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_, corners, ids, detectorParams_, rejected); + cv::aruco::detectMarkers(image, dictionary_, cvCorners, cvIds, detectorParams_, cvRejected); #else - cv::aruco::detectMarkers(image, *dictionary_, corners, ids, *detectorParams_, rejected); + cv::aruco::detectMarkers(image, *dictionary_, cvCorners, cvIds, *detectorParams_, cvRejected); #endif - cv::aruco::estimatePoseSingleMarkers(corners, markerLength_<=0.0?1.0f:markerLength_, model.K(), model.D(), rvecs, tvecs); + UDEBUG("Markers detected=%d rejected=%d", (int)cvIds.size(), (int)cvRejected.size()); - for(size_t i=0; i idsAdded; + std::vector< std::vector< std::vector< cv::Point2f > > > cvCornersPerCam(models.size()); + std::vector< std::vector< int > > cvIdsPerCam(models.size()); + for(size_t i=0; i(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); + int id = cvIds[i]; + + // Which camera? + int cameraIndex = int(cvCorners[i][0].x) / subRGBWidth; + UASSERT(cameraIndex>=0 && cameraIndex<(int)models.size()); + + 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 rvecs, tvecs; + const CameraModel & model = models[cam]; + cv::aruco::estimatePoseSingleMarkers(cvCornersPerCam[cam], 1.0f, model.K(), model.D(), rvecs, tvecs); + float offsetX = cam*subRGBWidth; + for(size_t i=0; i(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 0) + // Estimate scale and fill up output detections + std::vector scales; + std::map detections; + for(size_t i=0; i::const_iterator findIter = markerLengths.find(ids[i]); + if(!depth.empty() && (markerLength_ == 0 || (markerLength_<0 && findIter==markerLengths.end()))) { - rgbToDepthFactorX = 1.0f/(model.imageWidth()>0?float(model.imageWidth())/float(depth.cols):1.0f); - rgbToDepthFactorY = 1.0f/(model.imageHeight()>0?float(model.imageHeight())/float(depth.rows):1.0f); - } - else if(markerLength_ == 0) - { - if(depth.empty()) - { - UERROR("Depth image is empty, please set %s parameter to non-null.", Parameters::kMarkerLength().c_str()); - return detections; - } - } - - std::vector scales; - for(size_t i=0; i::const_iterator findIter = markerLengths.find(ids[i]); - if(!depth.empty() && (markerLength_ == 0 || (markerLength_<0 && findIter==markerLengths.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 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_) { - 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); - poses[i].x() *= length; - poses[i].y() *= length; - poses[i].z() *= length; - UWARN("Automatic marker length estimation: id=%d depth=%fm length=%fm", ids[i], d, length); - } - 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; - } + 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; - poses[i].x() *= length; - poses[i].y() *= length; - poses[i].z() *= 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 || poses[i].z() < maxRange_) && - (minRange_ <= 0 || poses[i].z() > minRange_)) + else { - Transform pose = model.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(), 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(markerLength_ < 0) { - float sum = 0.0f; - float maxError = 0.0f; - for(size_t i=0; i0) + length = findIter->second; + } + 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; + } + + 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) { - 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; - } + 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); + 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) @@ -507,13 +537,16 @@ std::map MarkerDetector::detect(const cv::Mat & image, std::map::iterator iter = detections.find(ids[i]); if(iter!=detections.end()) { + int cam = cams[i]; + const CameraModel & model = models[cam]; + cv::Mat subImage(*imageWithDetections, cv::Rect(subRGBWidth*i, 0, subRGBWidth, imageWithDetections->rows)); cv::Vec3d rvec; cv::Vec3d tvec(poses[i].x(), poses[i].y(), poses[i].z()); cv::Rodrigues(poses[i].rotationMatrix(), 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(), rvec, tvec, iter->second.length() * 0.5f); + cv::drawFrameAxes(subImage, model.K(), model.D(), rvec, tvec, iter->second.length() * 0.5f); #else - cv::aruco::drawAxis(*imageWithDetections, model.K(), model.D(), rvec, tvec, iter->second.length() * 0.5f); + cv::aruco::drawAxis(subImage, model.K(), model.D(), rvec, tvec, iter->second.length() * 0.5f); #endif } } @@ -526,5 +559,23 @@ std::map MarkerDetector::detect(const cv::Mat & image, return detections; } +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(); + } + + std::vector models; + models.push_back(model); + + return detect(image, models, depth, markerLengths, imageWithDetections); +} + } /* namespace rtabmap */ diff --git a/guilib/src/PreferencesDialog.cpp b/guilib/src/PreferencesDialog.cpp index b71a39a219..7e67b6079f 100644 --- a/guilib/src/PreferencesDialog.cpp +++ b/guilib/src/PreferencesDialog.cpp @@ -1750,7 +1750,8 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) : _ui->stereosgbm_mode->setObjectName(Parameters::kStereoSGBMMode().c_str()); // Aruco marker - _ui->ArucoDictionary->setObjectName(Parameters::kMarkerDictionary().c_str()); + _ui->MarkerStrategy->setObjectName(Parameters::kMarkerStrategy().c_str()); + _ui->MarkerDictionary->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()); diff --git a/guilib/src/ui/preferencesDialog.ui b/guilib/src/ui/preferencesDialog.ui index 36972d7082..63b979d378 100644 --- a/guilib/src/ui/preferencesDialog.ui +++ b/guilib/src/ui/preferencesDialog.ui @@ -95,7 +95,7 @@ QFrame::Raised - 14 + 18 @@ -15494,30 +15494,21 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - - - m - - - 2 - - - 0.000000000000000 - - - 999.000000000000000 + + + + Maximum detection range (0=unlimited). - - 1.000000000000000 + + true - - 0.000000000000000 + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - + + @@ -15538,29 +15529,36 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - - - m + + + + 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). - - 4 + + true - - -1.000000000000000 + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - 0.010000000000000 + + + + + + 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. - - 0.100000000000000 + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - + - 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). + Minimum detection range (0=disabled). true @@ -15570,10 +15568,10 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - + + - Minimum detection range (0=disabled). + Angular variance to set on marker priors. true @@ -15583,32 +15581,29 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - + + - + m - 6 + 4 - 0.000001000000000 - - - 9999.000000000000000 + 0.000100000000000 - 0.001000000000000 + 0.010000000000000 - 0.001000000000000 + 0.100000000000000 - - + + - Linear variance to set on marker priors. + Dictionary to use. true @@ -15618,10 +15613,139 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - + + + + + + + + + + + + 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 + + + + + + - Angular variance to set on marker priors. + Marker length. 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. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + 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,83 +15762,71 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - - - - - - 6 - - - 0.000001000000000 + + + + Linear variance to set on marker priors. - - 0.001000000000000 + + true - - 0.001000000000000 + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - + + m - 4 + 2 - 0.000100000000000 + 0.000000000000000 + + + 999.000000000000000 - 0.010000000000000 + 1.000000000000000 - 0.100000000000000 + 0.000000000000000 - - + + - m + - 2 + 6 - 0.000000000000000 - - - 999.000000000000000 + 0.000001000000000 - 1.000000000000000 + 0.001000000000000 - 0.000000000000000 + 0.001000000000000 - - + + - 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 - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + - - + + - 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. + 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 @@ -15724,10 +15836,13 @@ 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). + - 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). + Adjust variance to ignore orientation during optimization. Mouseover for more details. true @@ -15737,8 +15852,8 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - + + @@ -15748,6 +15863,9 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag 0.000001000000000 + + 9999.000000000000000 + 0.001000000000000 @@ -15756,10 +15874,10 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - + + - Maximum detection range (0=unlimited). + Detect static markers to be added as landmarks for graph optimization. If input data have already landmarks, this will be ignored. true @@ -15769,46 +15887,84 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - - + + + + + 6 + + + 0.000001000000000 + + + 0.001000000000000 + + + 0.001000000000000 + - - - - 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). + + + + m - - true + + 2 - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + 0.000000000000000 + + + 999.000000000000000 + + + 1.000000000000000 + + + 0.000000000000000 - - - - Detect static markers to be added as landmarks for graph optimization. If input data have already landmarks, this will be ignored. + + + + m - - true + + 4 - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + -1.000000000000000 + + + 0.010000000000000 + + + 0.100000000000000 - - - - 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). - + + + + + opencv-aruco + + + + + apriltag + + + + + + - Adjust variance to ignore orientation during optimization. Mouseover for more details. + Detector implementation. Note that both aruco and apriltag dictionaries can be used with either strategy. true @@ -15818,134 +15974,42 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - - - - - - - ArUco + OpenCV - - - - - 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 + None - APRILTAG_25h9 + Subpixel - APRILTAG_36h10 + Contour - APRILTAG_36h11 + AprilTag 2 - - + + - Dictionary to use. + Corner refinement method. true @@ -15955,34 +16019,19 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - - - - None - - - - - Subpixel - - - - - Contour - - - - - AprilTag 2 - - - - - - + + + + + + + AprilTag + + + + - Corner refinement method. + nthreads. true @@ -15992,6 +16041,9 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag + + + From 531eee635704b8e4fb7fff0621577b4f1a7ee606 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Thu, 14 May 2026 19:29:16 -0700 Subject: [PATCH 03/19] Exposed all AprilTag parameters. Sharing Marker/Dictionary between opencv and apriltag implementation. Added new Marker/Lengths parameter. --- CMakeLists.txt | 2 +- corelib/include/rtabmap/core/MarkerDetector.h | 1 + corelib/include/rtabmap/core/Parameters.h | 13 +- corelib/src/CMakeLists.txt | 4 +- corelib/src/MarkerDetector.cpp | 313 ++++- corelib/src/Parameters.cpp | 11 +- guilib/src/AboutDialog.cpp | 7 + guilib/src/PreferencesDialog.cpp | 55 +- guilib/src/ui/aboutDialog.ui | 1069 +++++++++-------- guilib/src/ui/preferencesDialog.ui | 623 ++++++---- 10 files changed, 1307 insertions(+), 791 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index b06708167e..a07cde3b50 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}) diff --git a/corelib/include/rtabmap/core/MarkerDetector.h b/corelib/include/rtabmap/core/MarkerDetector.h index 75e53b5398..630bf87f3e 100644 --- a/corelib/include/rtabmap/core/MarkerDetector.h +++ b/corelib/include/rtabmap/core/MarkerDetector.h @@ -92,6 +92,7 @@ class RTABMAP_CORE_EXPORT MarkerDetector { private: Strategy strategy_; float markerLength_; + std::map markerLengths_; float maxDepthError_; float maxRange_; float minRange_; diff --git a/corelib/include/rtabmap/core/Parameters.h b/corelib/include/rtabmap/core/Parameters.h index f3614d392e..8418854d87 100644 --- a/corelib/include/rtabmap/core/Parameters.h +++ b/corelib/include/rtabmap/core/Parameters.h @@ -918,19 +918,28 @@ class RTABMAP_CORE_EXPORT Parameters 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, 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"); + 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), markers are separated by a vertical line (\"id1 length|id2 length\"). If empty, all markers of the chosen dictionary can be detected and their length is set/estimated based on %s. For example, to detect only markers 12 and 14 with lengths of 8 and 15 cm respectively, set \"12 0.08|14 0.15\".", kMarkerDictionary().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(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, 2.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/src/CMakeLists.txt b/corelib/src/CMakeLists.txt index 253499d67f..5f2363a98c 100644 --- a/corelib/src/CMakeLists.txt +++ b/corelib/src/CMakeLists.txt @@ -534,8 +534,8 @@ IF(FastCV_FOUND) ENDIF(FastCV_FOUND) IF(apriltag_FOUND) - SET(LIBRARIES - ${LIBRARIES} + SET(PUBLIC_LIBRARIES + ${PUBLIC_LIBRARIES} apriltag::apriltag ) ENDIF(apriltag_FOUND) diff --git a/corelib/src/MarkerDetector.cpp b/corelib/src/MarkerDetector.cpp index 2a8f890f5d..c00a55d505 100644 --- a/corelib/src/MarkerDetector.cpp +++ b/corelib/src/MarkerDetector.cpp @@ -28,17 +28,163 @@ 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" +#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/tag16h5.h" +#include "apriltag/tag25h9.h" +#include "apriltag/tag36h10.h" #include "apriltag/tag36h11.h" +#include "apriltag/aruco/tagArucoMIP36h12.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 { +#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) + { + 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) + APRILTAG_CASE(16, 5) + APRILTAG_CASE(25, 9) + APRILTAG_CASE(36, 10) + APRILTAG_CASE(36, 11) + case cv::aruco::DICT_ARUCO_MIP_36h12: + af = tagArucoMIP36h12_create(); + break; + + default: + break; + } + return af; +} +void destroyAprilTagDictionary(apriltag_family_t * dictionary) +{ + if(dictionary == NULL) + { + return; + } + const char* name = dictionary->name; + 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, "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); + else if(strcmp(name, "tagArucoMIP_36h12") == 0) tagArucoMIP36h12_destroy(dictionary); + else + { + UFATAL("AprilTag: Didn't find the right desctructor for dictionary \"%s\"", name); + } +} +#endif + MarkerDetector::MarkerDetector(const ParametersMap & parameters) : strategy_((Strategy)Parameters::defaultMarkerStrategy()), markerLength_(Parameters::defaultMarkerLength()), @@ -58,11 +204,11 @@ MarkerDetector::MarkerDetector(const ParametersMap & parameters) : detectorParams_.reset(new cv::aruco::DetectorParameters()); #endif #if CV_MAJOR_VERSION > 4 || (CV_MAJOR_VERSION == 4 && CV_MINOR_VERSION >= 7) - detectorParams_->cornerRefinementMethod = (cv::aruco::CornerRefineMethod) Parameters::defaultMarkerCornerRefinementMethod(); + detectorParams_->cornerRefinementMethod = (cv::aruco::CornerRefineMethod) Parameters::defaultMarkerOpenCVCornerRefinementMethod(); #elif CV_MAJOR_VERSION > 3 || (CV_MAJOR_VERSION == 3 && CV_MINOR_VERSION >=3) - detectorParams_->cornerRefinementMethod = Parameters::defaultMarkerCornerRefinementMethod(); + detectorParams_->cornerRefinementMethod = Parameters::defaultMarkerOpenCVCornerRefinementMethod(); #else - detectorParams_->doCornerRefinement = Parameters::defaultMarkerCornerRefinementMethod()!=0; + detectorParams_->doCornerRefinement = Parameters::defaultMarkerOpenCVCornerRefinementMethod()!=0; #endif #endif @@ -77,7 +223,7 @@ MarkerDetector::~MarkerDetector() { } if(apriltagLibFamily_) { - tag36h11_destroy((apriltag_family_t*)apriltagLibFamily_); + destroyAprilTagDictionary((apriltag_family_t*)apriltagLibFamily_); } #endif } @@ -92,6 +238,36 @@ void MarkerDetector::parseParameters(const ParametersMap & parameters) 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 + { + int id = uStr2Int(items.front()); + float length = uStr2Float(items.back()); + UDEBUG("Adding marker %d with length %f", id, length); + markerLengths_.insert(std::make_pair(id, length)); + } + } + } #ifdef HAVE_OPENCV_ARUCO detectorParams_->adaptiveThreshWinSizeMin = 3; @@ -106,13 +282,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; @@ -127,7 +303,7 @@ void MarkerDetector::parseParameters(const ParametersMap & parameters) #if CV_MAJOR_VERSION < 3 || (CV_MAJOR_VERSION == 3 && (CV_MINOR_VERSION <4 || (CV_MINOR_VERSION ==4 && CV_SUBMINOR_VERSION<2))) - if(dictionaryId_ >= 17 && strategy_ == 0) + 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)", @@ -136,37 +312,67 @@ 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 #endif #ifdef RTABMAP_APRILTAG - if(apriltagLibDetector_) - { - apriltag_detector_destroy(((apriltag_detector_t*)apriltagLibDetector_)); - apriltagLibDetector_ = NULL; + if(apriltagLibDetector_ && apriltagLibFamily_) { + apriltag_detector_remove_family(((apriltag_detector_t*)apriltagLibDetector_), (apriltag_family_t*)apriltagLibFamily_); } if(apriltagLibFamily_) { - tag36h11_destroy((apriltag_family_t*)apriltagLibFamily_); + destroyAprilTagDictionary((apriltag_family_t*)apriltagLibFamily_); apriltagLibFamily_ = NULL; } - apriltagLibDetector_ = apriltag_detector_create(); - ((apriltag_detector_t*)apriltagLibDetector_)->nthreads = 2; - ((apriltag_detector_t*)apriltagLibDetector_)->quad_decimate = 1; - apriltagLibFamily_ = tag36h11_create(); - 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."); + 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_); + if(dictFamily == cv::aruco::DICT_ARUCO_ORIGINAL) + { + UERROR("Cannot set ARUCO_ORIGINAL dictionary with AprilTag library implementation. " + "Use opencv-aruco implementation instead. Setting %s to default (%d)", + Parameters::kMarkerDictionary().c_str(), + Parameters::defaultMarkerDictionary()); + dictionaryId_ = Parameters::defaultMarkerDictionary(); + 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()); + 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."); + } } #endif } @@ -193,7 +399,7 @@ 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()); @@ -226,8 +432,12 @@ std::map MarkerDetector::detect(const cv::Mat & image, if(strategy_ == kStrategyApriltag) { #ifdef RTABMAP_APRILTAG - UASSERT(image.type() == CV_8UC1); - image_u8_t im = {image.cols, image.rows, (int)image.step, image.data}; + cv::Mat grayImage = image; + if(image.channels() > 1) + { + cv::cvtColor(image, grayImage, cv::COLOR_BGR2GRAY); + } + image_u8_t im = {grayImage.cols, grayImage.rows, (int)grayImage.step, grayImage.data}; zarray_t *apriltagDetections = apriltag_detector_detect(((apriltag_detector_t*)apriltagLibDetector_), &im); @@ -250,6 +460,15 @@ std::map MarkerDetector::detect(const cv::Mat & image, int cameraIndex = int(det->c[0]) / subRGBWidth; UASSERT(cameraIndex>=0 && cameraIndex<(int)models.size()); + if(!markerLengths_.empty() && + markerLengths_.find(det->id) == markerLengths_.end() && + extraMarkerLengths.find(det->id) == extraMarkerLengths.end()) + { + UDEBUG("Ignoring marker %d because it is not in the list of expected markers (see %s)", + det->id, + Parameters::kMarkerLengths().c_str()); + continue; + } if(idsAdded.find(det->id)!=idsAdded.end()) { UWARN("Marker %d already added by another camera, ignoring detection from camera %d", det->id, cameraIndex); @@ -337,6 +556,15 @@ std::map MarkerDetector::detect(const cv::Mat & image, int cameraIndex = int(cvCorners[i][0].x) / subRGBWidth; UASSERT(cameraIndex>=0 && cameraIndex<(int)models.size()); + 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); @@ -390,8 +618,8 @@ std::map MarkerDetector::detect(const cv::Mat & image, for(size_t i=0; i::const_iterator findIter = markerLengths.find(ids[i]); - if(!depth.empty() && (markerLength_ == 0 || (markerLength_<0 && findIter==markerLengths.end()))) + std::map::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); @@ -438,18 +666,41 @@ std::map MarkerDetector::detect(const cv::Mat & image, continue; } } - else if(markerLength_ < 0) + else if(!markerLengths_.empty() || findIter!=extraMarkerLengths.end()) { - if(findIter!=markerLengths.end()) + std::map::const_iterator paramIter = markerLengths_.find(ids[i]); + if(paramIter != markerLengths_.end()) + { + if(findIter!=extraMarkerLengths.end() && findIter->second != paramIter->second) + { + UWARN("Marker's length of %d is defined both in extra lengths " + "(%f m) and the parameter %s (%f m), we will use the length " + "from extra lengths.", + findIter->second, + Parameters::kMarkerLengths().c_str(), + paramIter->second); + length = findIter->second; + } + else + { + length = paramIter->second; + } + } + else if(findIter!=extraMarkerLengths.end()) { length = findIter->second; } else { - UWARN("Cannot find marker length for marker %d, ignoring this marker (count=%d)", ids[i], (int)markerLengths.size()); + 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_; 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/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/PreferencesDialog.cpp b/guilib/src/PreferencesDialog.cpp index 987a435284..390d6c12e3 100644 --- a/guilib/src/PreferencesDialog.cpp +++ b/guilib/src/PreferencesDialog.cpp @@ -476,14 +476,23 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) : #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(RTABMAP_APRILTAG) && (CV_MAJOR_VERSION < 3 || (CV_MAJOR_VERSION == 3 && (CV_MINOR_VERSION <4 || (CV_MINOR_VERSION ==4 && CV_SUBMINOR_VERSION<2)))) + _ui->MarkerDictionary->setItemData(17, 0, Qt::UserRole - 1); + _ui->MarkerDictionary->setItemData(18, 0, Qt::UserRole - 1); + _ui->MarkerDictionary->setItemData(19, 0, Qt::UserRole - 1); + _ui->MarkerDictionary->setItemData(20, 0, Qt::UserRole - 1); +#endif +#if !defined(RTABMAP_APRILTAG) && (CV_MAJOR_VERSION < 4 || (CV_MAJOR_VERSION == 4 && CV_MINOR_VERSION <8)) + _ui->MarkerDictionary->setItemData(21, 0, Qt::UserRole - 1); +#endif +#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 @@ -1752,6 +1761,8 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) : // Aruco marker _ui->MarkerStrategy->setObjectName(Parameters::kMarkerStrategy().c_str()); + connect(_ui->MarkerStrategy, SIGNAL(currentIndexChanged(int)), _ui->stackedWidget_markerStrategy, SLOT(setCurrentIndex(int))); + _ui->MarkerStrategy->setCurrentIndex(Parameters::defaultMarkerStrategy()); _ui->MarkerDictionary->setObjectName(Parameters::kMarkerDictionary().c_str()); _ui->ArucoMarkerLength->setObjectName(Parameters::kMarkerLength().c_str()); _ui->ArucoMaxDepthError->setObjectName(Parameters::kMarkerMaxDepthError().c_str()); @@ -1763,7 +1774,13 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) : _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->ArucoCornerRefinementMethod->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()); @@ -3939,15 +3956,25 @@ 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-aruco: 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 - + } return true; } 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 64c9465f0b..6ed3a0a197 100644 --- a/guilib/src/ui/preferencesDialog.ui +++ b/guilib/src/ui/preferencesDialog.ui @@ -63,7 +63,7 @@ 0 - -700 + -526 684 5218 @@ -15505,7 +15505,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - + @@ -15514,21 +15514,29 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - + + + + + opencv-aruco + + + + + apriltag + + + + + + - Maximum detection range (0=unlimited). - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + - - + + @@ -15538,9 +15546,6 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag 0.000001000000000 - - 9999.000000000000000 - 0.001000000000000 @@ -15549,7 +15554,26 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - + + + + m + + + 4 + + + -1.000000000000000 + + + 0.010000000000000 + + + 0.100000000000000 + + + + 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). @@ -15562,10 +15586,32 @@ 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 + + + + + - 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. + 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 @@ -15575,10 +15621,10 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - + + - Minimum detection range (0=disabled). + Marker length. 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. true @@ -15589,9 +15635,9 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - + - Angular variance to set on marker priors. + Linear variance to set on marker priors. true @@ -15601,42 +15647,25 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - + + - m + - 4 + 6 - 0.000100000000000 + 0.000001000000000 + + + 9999.000000000000000 - 0.010000000000000 + 0.001000000000000 - 0.100000000000000 - - - - - - - Dictionary to use. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - + 0.001000000000000 @@ -15749,10 +15778,10 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - + + - Marker length. 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. + Dictionary to use. true @@ -15762,10 +15791,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). + Detect static markers to be added as landmarks for graph optimization. If input data have already landmarks, this will be ignored. true @@ -15775,17 +15804,46 @@ 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. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + - - + + - Linear variance to set on marker priors. + Angular variance to set on marker priors. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + 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 @@ -15796,7 +15854,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - + m @@ -15817,33 +15875,33 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - - - - - - 6 - - - 0.000001000000000 + + + + Minimum detection range (0=disabled). - - 0.001000000000000 + + true - - 0.001000000000000 + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - + + - + Maximum detection range (0=unlimited). + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - + 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. @@ -15856,24 +15914,34 @@ 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). + + + + m - - Adjust variance to ignore orientation during optimization. Mouseover for more details. + + 4 - - true + + 0.000100000000000 - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + 0.010000000000000 + + + 0.100000000000000 - - + + + + + + + + + @@ -15883,9 +15951,6 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag 0.000001000000000 - - 9999.000000000000000 - 0.001000000000000 @@ -15894,10 +15959,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. + Detector implementation. Note that both aruco and apriltag dictionaries can be used with either strategy. true @@ -15907,8 +15972,8 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - + + @@ -15918,73 +15983,28 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag 0.000001000000000 - - 0.001000000000000 - - - 0.001000000000000 - - - - - - - m - - - 2 - - - 0.000000000000000 - - 999.000000000000000 + 9999.000000000000000 - 1.000000000000000 + 0.001000000000000 - 0.000000000000000 + 0.001000000000000 - - - - m - - - 4 - - - -1.000000000000000 - - - 0.010000000000000 - - - 0.100000000000000 + + + + - - - - - opencv-aruco - - - - - apriltag - - - - - - + + - Detector implementation. Note that both aruco and apriltag dictionaries can be used with either strategy. + 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"). If empty, all markers of the chosen dictionary can be detected and their length is set/estimated based on Marker Length. For example, to detect only markers 12 and 14 with lengths of 8 and 15 cm respectively, set "12 0.08|14 0.15". true @@ -15997,74 +16017,233 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - - OpenCV + + + 1 - - - - - - None - - - - - Subpixel - - - - - Contour + + + + + + OpenCV - - - - AprilTag 2 + + + + + Corner refinement method. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + None + + + + + Subpixel + + + + + Contour + + + + + AprilTag 2 + + + + + + + + Qt::Vertical + + + + + + + + + + + + + + AprilTag - - - - - - - Corner refinement method. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - - - - AprilTag - - - - - - nthreads. - - - 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 + + + 2 + + + + + + + + From 2e00532c7d26bf04f48d8901a6323fef998ce907 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Thu, 14 May 2026 22:13:30 -0700 Subject: [PATCH 04/19] Support AprilTAg library not built with aruco --- CMakeLists.txt | 14 ++- Version.h.in | 1 + corelib/src/CMakeLists.txt | 4 +- corelib/src/MarkerDetector.cpp | 68 ++++++++++--- .../include/rtabmap/gui/PreferencesDialog.h | 1 + guilib/src/PreferencesDialog.cpp | 97 ++++++++++++++----- guilib/src/ui/preferencesDialog.ui | 45 +++++---- 7 files changed, 170 insertions(+), 60 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index a07cde3b50..aa45d735c5 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -870,7 +870,14 @@ ENDIF(WITH_FASTCV) IF(WITH_APRILTAG) FIND_PACKAGE(apriltag QUIET) IF(apriltag_FOUND) - MESSAGE(STATUS "Found apriltag") + 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 False) + IF(apriltag_aruco_4x4_50_header) + SET(WITH_APRILTAG_ARUCO True) + ENDIF() + MESSAGE(STATUS "Found apriltag (with aruco=${WITH_APRILTAG_ARUCO}): ${APRILTAG_LOCATION} ${APRILTAG_INCLUDES}") ENDIF(apriltag_FOUND) ENDIF(WITH_APRILTAG) @@ -1076,7 +1083,10 @@ IF(NOT FastCV_FOUND) ENDIF(NOT FastCV_FOUND) IF(NOT apriltag_FOUND) SET(APRILTAG "//") -ENDIF(NOT apriltag_FOUND) + 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) diff --git a/Version.h.in b/Version.h.in index 714761a727..2bb9d46af1 100644 --- a/Version.h.in +++ b/Version.h.in @@ -94,6 +94,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. @PYTHON@#define RTABMAP_PYTHON @MADGWICK@#define RTABMAP_MADGWICK @APRILTAG@#define RTABMAP_APRILTAG +@APRILTAG_ARUCO@#define RTABMAP_APRILTAG_WITH_ARUCO #include diff --git a/corelib/src/CMakeLists.txt b/corelib/src/CMakeLists.txt index 5f2363a98c..8f82257b96 100644 --- a/corelib/src/CMakeLists.txt +++ b/corelib/src/CMakeLists.txt @@ -534,9 +534,9 @@ IF(FastCV_FOUND) ENDIF(FastCV_FOUND) IF(apriltag_FOUND) - SET(PUBLIC_LIBRARIES - ${PUBLIC_LIBRARIES} + SET(LIBRARIES apriltag::apriltag + ${LIBRARIES} ) ENDIF(apriltag_FOUND) diff --git a/corelib/src/MarkerDetector.cpp b/corelib/src/MarkerDetector.cpp index c00a55d505..8d67d50eba 100644 --- a/corelib/src/MarkerDetector.cpp +++ b/corelib/src/MarkerDetector.cpp @@ -47,6 +47,7 @@ namespace cv{ 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" @@ -63,11 +64,12 @@ extern "C" { #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" -#include "apriltag/aruco/tagArucoMIP36h12.h" } #ifndef HAVE_OPENCV_ARUCO @@ -121,6 +123,11 @@ 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) @@ -137,14 +144,10 @@ apriltag_family_t * createAprilTagPredefinedDictionary(int opencvArucoDictionary ARUCO_CASE(7, 100) ARUCO_CASE(7, 250) ARUCO_CASE(7, 1000) - APRILTAG_CASE(16, 5) - APRILTAG_CASE(25, 9) - APRILTAG_CASE(36, 10) - APRILTAG_CASE(36, 11) case cv::aruco::DICT_ARUCO_MIP_36h12: af = tagArucoMIP36h12_create(); break; - +#endif default: break; } @@ -157,7 +160,12 @@ void destroyAprilTagDictionary(apriltag_family_t * dictionary) return; } const char* name = dictionary->name; - if(strcmp(name, "tagAruco4x4_50") == 0) tagAruco4x4_50_destroy(dictionary); + 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); @@ -173,11 +181,8 @@ void destroyAprilTagDictionary(apriltag_family_t * 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, "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); else if(strcmp(name, "tagArucoMIP_36h12") == 0) tagArucoMIP36h12_destroy(dictionary); +#endif else { UFATAL("AprilTag: Didn't find the right desctructor for dictionary \"%s\"", name); @@ -332,6 +337,16 @@ void MarkerDetector::parseParameters(const ParametersMap & parameters) dictionary_.reset(new cv::aruco::Dictionary()); *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_ = 1; +#else + UERROR("RTAB-Map is not built with opencv-aruco library!"); +#endif + } #endif #ifdef RTABMAP_APRILTAG @@ -357,13 +372,26 @@ void MarkerDetector::parseParameters(const ParametersMap & parameters) 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 dictionary with AprilTag library implementation. " - "Use opencv-aruco implementation instead. Setting %s to default (%d)", + 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(), - Parameters::defaultMarkerDictionary()); - dictionaryId_ = Parameters::defaultMarkerDictionary(); + cv::aruco::DICT_APRILTAG_36h11); + dictionaryId_ = cv::aruco::DICT_APRILTAG_36h11; dictFamily = cv::aruco::PredefinedDictionaryType(dictionaryId_); } apriltagLibFamily_ = createAprilTagPredefinedDictionary(dictFamily); @@ -374,6 +402,16 @@ void MarkerDetector::parseParameters(const ParametersMap & parameters) 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_ = 0; +#else + UERROR("RTAB-Map is not built with apriltag library!"); +#endif + } #endif } 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/PreferencesDialog.cpp b/guilib/src/PreferencesDialog.cpp index 390d6c12e3..46941756dd 100644 --- a/guilib/src/PreferencesDialog.cpp +++ b/guilib/src/PreferencesDialog.cpp @@ -475,16 +475,6 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) : _ui->checkBox_showOdomFrustums->setChecked(false); #endif - //if OpenCV < 3.4.2 -#if !defined(RTABMAP_APRILTAG) && (CV_MAJOR_VERSION < 3 || (CV_MAJOR_VERSION == 3 && (CV_MINOR_VERSION <4 || (CV_MINOR_VERSION ==4 && CV_SUBMINOR_VERSION<2)))) - _ui->MarkerDictionary->setItemData(17, 0, Qt::UserRole - 1); - _ui->MarkerDictionary->setItemData(18, 0, Qt::UserRole - 1); - _ui->MarkerDictionary->setItemData(19, 0, Qt::UserRole - 1); - _ui->MarkerDictionary->setItemData(20, 0, Qt::UserRole - 1); -#endif -#if !defined(RTABMAP_APRILTAG) && (CV_MAJOR_VERSION < 4 || (CV_MAJOR_VERSION == 4 && CV_MINOR_VERSION <8)) - _ui->MarkerDictionary->setItemData(21, 0, Qt::UserRole - 1); -#endif #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 @@ -1762,19 +1752,22 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) : // Aruco marker _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->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::kMarkerOpenCVCornerRefinementMethod().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()); @@ -3970,9 +3963,20 @@ bool PreferencesDialog::validateForm() if(_ui->MarkerDictionary->currentIndex()>=21) { QMessageBox::warning(this, tr("Parameter warning"), - tr("opencv-aruco: cannot use selected dictionary (%1), OpenCV version should be at least 4.8.0. Setting back to 0.").arg(_ui->MarkerDictionary->currentIndex())); + 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; @@ -5601,6 +5605,53 @@ 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); +#if !defined(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 !defined(RTABMAP_APRILTAG) && (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()); + } +#endif +#if !defined(RTABMAP_APRILTAG_WITH_ARUCO) && (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 @@ -6265,7 +6316,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/preferencesDialog.ui b/guilib/src/ui/preferencesDialog.ui index 6ed3a0a197..96aa130730 100644 --- a/guilib/src/ui/preferencesDialog.ui +++ b/guilib/src/ui/preferencesDialog.ui @@ -63,7 +63,7 @@ 0 - -526 + 0 684 5218 @@ -15529,14 +15529,14 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - + - + @@ -15555,7 +15555,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - + m @@ -15587,7 +15587,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - + m @@ -15648,7 +15648,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - + @@ -15818,7 +15818,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - + @@ -15854,7 +15854,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - + m @@ -15915,7 +15915,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - + m @@ -15941,7 +15941,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - + @@ -15962,7 +15962,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - Detector implementation. Note that both aruco and apriltag dictionaries can be used with either strategy. + 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 @@ -15973,7 +15973,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - + @@ -15995,7 +15995,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - + @@ -16019,7 +16019,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - 1 + 0 @@ -16043,7 +16043,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - + None @@ -16071,6 +16071,12 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag Qt::Vertical + + + 0 + 0 + + @@ -16230,12 +16236,15 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - + + 1 + + 1.000000000000000 + - 2 + 2.000000000000000 From 6b41afe37bbecbf11e32a9f9b08dc78490e655f4 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Thu, 14 May 2026 22:15:08 -0700 Subject: [PATCH 05/19] cleanup --- guilib/src/PreferencesDialog.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/guilib/src/PreferencesDialog.cpp b/guilib/src/PreferencesDialog.cpp index 46941756dd..55eefc49f9 100644 --- a/guilib/src/PreferencesDialog.cpp +++ b/guilib/src/PreferencesDialog.cpp @@ -5616,7 +5616,7 @@ void PreferencesDialog::updateAvailableMarkerDictionaries() { // ARUCO_ORIGINAL not available with AprilTag lib _ui->MarkerDictionary->setItemData(16, 0, Qt::UserRole - 1); -#if !defined(RTABMAP_APRILTAG_WITH_ARUCO) +#ifndef RTABMAP_APRILTAG_WITH_ARUCO // disable all aruco dictionaries for(int i=0;i<17;++i) { _ui->MarkerDictionary->setItemData(i, 0, Qt::UserRole - 1); @@ -5631,7 +5631,7 @@ void PreferencesDialog::updateAvailableMarkerDictionaries() else //if(_ui->MarkerStrategy->currentIndex() == 0) // OpenCV Strategy is selected { //if OpenCV < 3.4.2 -#if !defined(RTABMAP_APRILTAG) && (CV_MAJOR_VERSION < 3 || (CV_MAJOR_VERSION == 3 && (CV_MINOR_VERSION <4 || (CV_MINOR_VERSION ==4 && CV_SUBMINOR_VERSION<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); @@ -5641,7 +5641,7 @@ void PreferencesDialog::updateAvailableMarkerDictionaries() _ui->MarkerDictionary->setCurrentIndex(Parameters::defaultMarkerDictionary()); } #endif -#if !defined(RTABMAP_APRILTAG_WITH_ARUCO) && (CV_MAJOR_VERSION < 4 || (CV_MAJOR_VERSION == 4 && CV_MINOR_VERSION <8)) +#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) From 344812f127826d361866cf1b5d3d1b5bd66bd013 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Fri, 15 May 2026 07:47:47 -0700 Subject: [PATCH 06/19] Added marker range support --- corelib/include/rtabmap/core/Parameters.h | 4 +- corelib/src/MarkerDetector.cpp | 46 ++++++++++++++++++++--- guilib/src/ui/preferencesDialog.ui | 13 +++++-- 3 files changed, 53 insertions(+), 10 deletions(-) diff --git a/corelib/include/rtabmap/core/Parameters.h b/corelib/include/rtabmap/core/Parameters.h index 8418854d87..54c9e2567b 100644 --- a/corelib/include/rtabmap/core/Parameters.h +++ b/corelib/include/rtabmap/core/Parameters.h @@ -920,14 +920,14 @@ class RTABMAP_CORE_EXPORT Parameters 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), markers are separated by a vertical line (\"id1 length|id2 length\"). If empty, all markers of the chosen dictionary can be detected and their length is set/estimated based on %s. For example, to detect only markers 12 and 14 with lengths of 8 and 15 cm respectively, set \"12 0.08|14 0.15\".", kMarkerDictionary().c_str()).c_str()); + 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\".", kMarkerDictionary().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, 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."); diff --git a/corelib/src/MarkerDetector.cpp b/corelib/src/MarkerDetector.cpp index 8d67d50eba..0c8be089a0 100644 --- a/corelib/src/MarkerDetector.cpp +++ b/corelib/src/MarkerDetector.cpp @@ -266,10 +266,46 @@ void MarkerDetector::parseParameters(const ParametersMap & parameters) } else { - int id = uStr2Int(items.front()); float length = uStr2Float(items.back()); - UDEBUG("Adding marker %d with length %f", id, length); - markerLengths_.insert(std::make_pair(id, length)); + 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); + } + } } } } @@ -342,7 +378,7 @@ void MarkerDetector::parseParameters(const ParametersMap & parameters) { #ifdef RTABMAP_APRILTAG UERROR("RTAB-Map is not built with opencv-aruco library! Fallback to AprilTag strategy (%s=1).", Parameters::kMarkerStrategy().c_str()); - strategy_ = 1; + strategy_ = kStrategyApriltag; #else UERROR("RTAB-Map is not built with opencv-aruco library!"); #endif @@ -407,7 +443,7 @@ void MarkerDetector::parseParameters(const ParametersMap & parameters) { #ifdef HAVE_OPENCV_ARUCO UERROR("RTAB-Map is not built with apriltag library! Fallback to OpenCV (%s=0).", Parameters::kMarkerStrategy().c_str()); - strategy_ = 0; + strategy_ = kStrategyOpencv; #else UERROR("RTAB-Map is not built with apriltag library!"); #endif diff --git a/guilib/src/ui/preferencesDialog.ui b/guilib/src/ui/preferencesDialog.ui index 96aa130730..39fd016b90 100644 --- a/guilib/src/ui/preferencesDialog.ui +++ b/guilib/src/ui/preferencesDialog.ui @@ -15556,6 +15556,10 @@ 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. + m @@ -15576,7 +15580,7 @@ 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). + 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 @@ -15624,7 +15628,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - Marker length. 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. + 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 the spinbox to see the difference between negative and 0. true @@ -15996,6 +16000,9 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag + + 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" + @@ -16004,7 +16011,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - 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"). If empty, all markers of the chosen dictionary can be detected and their length is set/estimated based on Marker Length. For example, to detect only markers 12 and 14 with lengths of 8 and 15 cm respectively, set "12 0.08|14 0.15". + 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 text box for example. true From 654d72a0fc08b0b4b4418c3aee7ac79eb4981cab Mon Sep 17 00:00:00 2001 From: matlabbe Date: Fri, 15 May 2026 13:24:58 -0700 Subject: [PATCH 07/19] Make apriltag detection in same orientation than opencv --- corelib/src/MarkerDetector.cpp | 8 ++++---- corelib/src/optimizer/OptimizerGTSAM.cpp | 2 +- guilib/src/MainWindow.cpp | 1 + 3 files changed, 6 insertions(+), 5 deletions(-) diff --git a/corelib/src/MarkerDetector.cpp b/corelib/src/MarkerDetector.cpp index 0c8be089a0..88841215b5 100644 --- a/corelib/src/MarkerDetector.cpp +++ b/corelib/src/MarkerDetector.cpp @@ -574,10 +574,10 @@ std::map MarkerDetector::detect(const cv::Mat & image, double err = estimate_tag_pose(&info, &pose); if (pose.R && pose.t) { - Transform t(MATD_EL(pose.R, 0, 0), MATD_EL(pose.R, 1, 0), MATD_EL(pose.R, 2, 0), MATD_EL(pose.t, 0, 0), - MATD_EL(pose.R, 0, 1), MATD_EL(pose.R, 1, 1), MATD_EL(pose.R, 2, 1), MATD_EL(pose.t, 1, 0), - MATD_EL(pose.R, 0, 2), MATD_EL(pose.R, 1, 2), MATD_EL(pose.R, 2, 2), MATD_EL(pose.t, 2, 0)); - poses.push_back(t); + 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) diff --git a/corelib/src/optimizer/OptimizerGTSAM.cpp b/corelib/src/optimizer/OptimizerGTSAM.cpp index 56ac51ba28..e1b8a6892e 100644 --- a/corelib/src/optimizer/OptimizerGTSAM.cpp +++ b/corelib/src/optimizer/OptimizerGTSAM.cpp @@ -501,7 +501,7 @@ 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)) { diff --git a/guilib/src/MainWindow.cpp b/guilib/src/MainWindow.cpp index 92a053f2d5..e166a5bf0b 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 From 632d8504c9ebcf69c9ee244a633dd9caad1523c3 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Fri, 15 May 2026 13:59:41 -0700 Subject: [PATCH 08/19] fixed shenanigans when rendering markers with multi cameras --- guilib/src/MainWindow.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/guilib/src/MainWindow.cpp b/guilib/src/MainWindow.cpp index e166a5bf0b..8daf26b23f 100644 --- a/guilib/src/MainWindow.cpp +++ b/guilib/src/MainWindow.cpp @@ -5296,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) From 81c08570cd5f2ae96fab8adc73e39130176c4bb4 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Fri, 15 May 2026 14:34:55 -0700 Subject: [PATCH 09/19] Don't update odom cache (localization mode) when not moving (detecting loop/landmark or not) --- corelib/src/Rtabmap.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/corelib/src/Rtabmap.cpp b/corelib/src/Rtabmap.cpp index 13652171f5..fcaa4656b6 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();) From 0b2a4656ab2129ab0de4566831e08cc192b4be73 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Mon, 18 May 2026 10:53:52 -0700 Subject: [PATCH 10/19] fixed some quirks (camera viewer + tag working) --- CMakeLists.txt | 19 ++++++++++------ corelib/src/MarkerDetector.cpp | 27 ++++++++++++++--------- guilib/include/rtabmap/gui/CameraViewer.h | 1 + guilib/src/CameraViewer.cpp | 16 +++++++++++--- guilib/src/PreferencesDialog.cpp | 11 +++++++++ guilib/src/ui/preferencesDialog.ui | 18 +++++++-------- 6 files changed, 63 insertions(+), 29 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index aa45d735c5..47139b2b50 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1496,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() diff --git a/corelib/src/MarkerDetector.cpp b/corelib/src/MarkerDetector.cpp index 88841215b5..d0e587dc96 100644 --- a/corelib/src/MarkerDetector.cpp +++ b/corelib/src/MarkerDetector.cpp @@ -432,8 +432,8 @@ void MarkerDetector::parseParameters(const ParametersMap & parameters) } 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."); } @@ -454,6 +454,7 @@ void MarkerDetector::parseParameters(const ParametersMap & parameters) // 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); @@ -513,8 +514,8 @@ std::map MarkerDetector::detect(const cv::Mat & image, } 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) @@ -844,7 +845,6 @@ std::map MarkerDetector::detect(const cv::Mat & image, if(imageWithDetections) { -#ifdef HAVE_OPENCV_ARUCO if(image.channels()==1) { cv::cvtColor(image, *imageWithDetections, cv::COLOR_GRAY2BGR); @@ -853,32 +853,38 @@ 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]; - cv::Mat subImage(*imageWithDetections, cv::Rect(subRGBWidth*i, 0, subRGBWidth, imageWithDetections->rows)); + 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::Rodrigues(poses[i].rotationMatrix(), rvec); + 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(subImage, model.K(), model.D(), rvec, tvec, iter->second.length() * 0.5f); -#else +#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. Cannot draw markers on image."); -#endif } return detections; @@ -890,6 +896,7 @@ std::map MarkerDetector::detect(const cv::Mat & image, 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."); 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/src/CameraViewer.cpp b/guilib/src/CameraViewer.cpp index 601cbc6fe3..8c25121261 100644 --- a/guilib/src/CameraViewer.cpp +++ b/guilib/src/CameraViewer.cpp @@ -84,7 +84,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 @@ -165,12 +165,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 { @@ -236,7 +246,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/PreferencesDialog.cpp b/guilib/src/PreferencesDialog.cpp index 55eefc49f9..fd0e3cc0a7 100644 --- a/guilib/src/PreferencesDialog.cpp +++ b/guilib/src/PreferencesDialog.cpp @@ -5640,6 +5640,17 @@ void PreferencesDialog::updateAvailableMarkerDictionaries() { _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 diff --git a/guilib/src/ui/preferencesDialog.ui b/guilib/src/ui/preferencesDialog.ui index 39fd016b90..88411ab5db 100644 --- a/guilib/src/ui/preferencesDialog.ui +++ b/guilib/src/ui/preferencesDialog.ui @@ -15556,10 +15556,6 @@ 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. - m @@ -15627,8 +15623,12 @@ With <0, the length is estimated once for each unique marker, then re-used fo + + 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. + - 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 the spinbox to see the difference between negative and 0. + 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 @@ -16000,9 +16000,6 @@ With <0, the length is estimated once for each unique marker, then re-used fo - - 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" - @@ -16010,8 +16007,11 @@ With <0, the length is estimated once for each unique marker, then re-used fo + + 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 text box for example. + 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 From 2956956da341824005f957a108a7584ab3721bf8 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Mon, 18 May 2026 11:45:20 -0700 Subject: [PATCH 11/19] Setting quad decimate to 1 by default --- corelib/include/rtabmap/core/Parameters.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/corelib/include/rtabmap/core/Parameters.h b/corelib/include/rtabmap/core/Parameters.h index 54c9e2567b..37c8f8549e 100644 --- a/corelib/include/rtabmap/core/Parameters.h +++ b/corelib/include/rtabmap/core/Parameters.h @@ -932,7 +932,7 @@ class RTABMAP_CORE_EXPORT Parameters 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, 2.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, 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."); From 82f2edac9636ded7948d861baaf587a25c31b341 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Tue, 19 May 2026 17:04:38 -0700 Subject: [PATCH 12/19] typo --- corelib/include/rtabmap/core/Parameters.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/corelib/include/rtabmap/core/Parameters.h b/corelib/include/rtabmap/core/Parameters.h index 37c8f8549e..3944acf6c5 100644 --- a/corelib/include/rtabmap/core/Parameters.h +++ b/corelib/include/rtabmap/core/Parameters.h @@ -920,7 +920,7 @@ class RTABMAP_CORE_EXPORT Parameters 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\".", kMarkerDictionary().c_str()).c_str()); + 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())); From dc21e6df7641e7927c15cb8c51b5ce68c31abdd4 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Thu, 21 May 2026 18:41:40 -0700 Subject: [PATCH 13/19] Implemented OptimizerG2O::loadGraph() --- .../rtabmap/core/optimizer/OptimizerG2O.h | 6 + corelib/src/Graph.cpp | 9 +- corelib/src/optimizer/OptimizerG2O.cpp | 384 ++++++++++++++++++ tools/Export/main.cpp | 17 +- 4 files changed, 407 insertions(+), 9 deletions(-) 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/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/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/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()); From 26af228804f2c7ffeb3b7b0f7d60c1feefe10440 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sat, 23 May 2026 15:23:33 -0700 Subject: [PATCH 14/19] added apriltag's aruco support info in the cmake config summary --- CMakeLists.txt | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 47139b2b50..60986d09ff 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -873,9 +873,9 @@ IF(WITH_APRILTAG) 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 False) + SET(WITH_APRILTAG_ARUCO NO) IF(apriltag_aruco_4x4_50_header) - SET(WITH_APRILTAG_ARUCO True) + SET(WITH_APRILTAG_ARUCO YES) ENDIF() MESSAGE(STATUS "Found apriltag (with aruco=${WITH_APRILTAG_ARUCO}): ${APRILTAG_LOCATION} ${APRILTAG_INCLUDES}") ENDIF(apriltag_FOUND) @@ -1598,7 +1598,7 @@ MESSAGE(STATUS " With FastCV = NO (FastCV not found)") ENDIF() IF(apriltag_FOUND) -MESSAGE(STATUS " With AprilTag ${apriltag_VERSION} = YES (License: BSD 2-Clause License)") +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() From d8f787de4d9a9dcef076b184328ce446905d3379 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Tue, 26 May 2026 11:38:07 -0700 Subject: [PATCH 15/19] fixed warning --- examples/WifiMapping/WifiThread.h | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) 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; From 64186123be3c4fa4a67ba99ed62b0c2f96e51d0d Mon Sep 17 00:00:00 2001 From: matlabbe Date: Tue, 26 May 2026 12:05:01 -0700 Subject: [PATCH 16/19] fixed isam2 assert when using landmarks --- .../rtabmap/core/optimizer/OptimizerGTSAM.h | 1 + corelib/src/optimizer/OptimizerGTSAM.cpp | 47 +++++++++++-------- 2 files changed, 28 insertions(+), 20 deletions(-) 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/optimizer/OptimizerGTSAM.cpp b/corelib/src/optimizer/OptimizerGTSAM.cpp index e1b8a6892e..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()); + 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()))); From 7309a1255f8c7a2a9df3ab73d5c2d5175ae5d894 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Tue, 26 May 2026 13:07:14 -0700 Subject: [PATCH 17/19] Fixed homography on multicam --- corelib/src/MarkerDetector.cpp | 13 +++++++++++-- 1 file changed, 11 insertions(+), 2 deletions(-) diff --git a/corelib/src/MarkerDetector.cpp b/corelib/src/MarkerDetector.cpp index d0e587dc96..4326878db5 100644 --- a/corelib/src/MarkerDetector.cpp +++ b/corelib/src/MarkerDetector.cpp @@ -185,7 +185,7 @@ void destroyAprilTagDictionary(apriltag_family_t * dictionary) #endif else { - UFATAL("AprilTag: Didn't find the right desctructor for dictionary \"%s\"", name); + UFATAL("AprilTag: Didn't find the right destructor for dictionary \"%s\"", name); } } #endif @@ -560,7 +560,15 @@ std::map MarkerDetector::detect(const cv::Mat & image, det->p[i][0] -= offsetX; } - // FIXME, homography needs to be updated + // Shift the homography to match the local-camera pixel frame: + // pre-multiply H by T = [[1,0,-offsetX],[0,1,0],[0,0,1]] so it + // stays consistent with the shifted corners. estimate_tag_pose() + // seeds its iterative refinement from H, so leaving it stale + // produces a bad initial guess (often triggers the AprilTag + // "more than one new minimum found" debug print). + MATD_EL(det->H, 0, 0) -= offsetX * MATD_EL(det->H, 2, 0); + MATD_EL(det->H, 0, 1) -= offsetX * MATD_EL(det->H, 2, 1); + MATD_EL(det->H, 0, 2) -= offsetX * MATD_EL(det->H, 2, 2); apriltag_detection_info_t info; info.det = det; @@ -573,6 +581,7 @@ std::map MarkerDetector::detect(const cv::Mat & image, // Then call estimate_tag_pose. apriltag_pose_t pose; double err = estimate_tag_pose(&info, &pose); + if (pose.R && pose.t) { 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), From ef1c7b91168a40af5e05e7610ed4217147033db3 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Tue, 26 May 2026 13:38:15 -0700 Subject: [PATCH 18/19] Adding marker detection time for convenience --- guilib/src/CameraViewer.cpp | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/guilib/src/CameraViewer.cpp b/guilib/src/CameraViewer.cpp index 8c25121261..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 @@ -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; @@ -194,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() && From 5b2b598e7c314d217660d8be915acc8c41a6b979 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Tue, 26 May 2026 16:21:20 -0700 Subject: [PATCH 19/19] Added missing MIP 36h12 tag family in UI --- corelib/src/MarkerDetector.cpp | 54 +++++++++++++++++++++--------- guilib/src/ui/preferencesDialog.ui | 5 +++ 2 files changed, 43 insertions(+), 16 deletions(-) diff --git a/corelib/src/MarkerDetector.cpp b/corelib/src/MarkerDetector.cpp index 4326878db5..db3a4fd62b 100644 --- a/corelib/src/MarkerDetector.cpp +++ b/corelib/src/MarkerDetector.cpp @@ -501,7 +501,7 @@ std::map MarkerDetector::detect(const cv::Mat & image, std::vector< int > ids; std::vector< int > cams; std::vector< std::vector< cv::Point2f > > corners; // in stitched image - std::vector poses; + std::vector poses; // in optical frame // detect markers and estimate pose if(strategy_ == kStrategyApriltag) @@ -533,7 +533,12 @@ std::map MarkerDetector::detect(const cv::Mat & image, // Which camera? int cameraIndex = int(det->c[0]) / subRGBWidth; - UASSERT(cameraIndex>=0 && cameraIndex<(int)models.size()); + if(cameraIndex<0 || cameraIndex>=(int)models.size()) + { + UWARN("Marker %d detected outside the image! camera index=%d (models=%ld subWidth=%d) marker center=(%f,%f). Ignoring...", + det->id, cameraIndex, models.size(), subRGBWidth, det->c[0], det->c[1]); + continue; + } if(!markerLengths_.empty() && markerLengths_.find(det->id) == markerLengths_.end() && @@ -584,22 +589,29 @@ std::map MarkerDetector::detect(const cv::Mat & image, if (pose.R && pose.t) { - 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) + 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) + 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 { - // reconvert in original stitched image - corners.back()[i].x = det->p[i][0]+offsetX; - corners.back()[i].y = det->p[i][1]; + UWARN("Skipping %d because its estimated pose is behind the camera %d", det->id, cameraIndex); } - ids.push_back(det->id); - cams.push_back(cameraIndex); - UDEBUG("Add marker %d (err = %f)", det->id, err); - idsAdded.insert(det->id); } else { @@ -638,7 +650,12 @@ std::map MarkerDetector::detect(const cv::Mat & image, // Which camera? int cameraIndex = int(cvCorners[i][0].x) / subRGBWidth; - UASSERT(cameraIndex>=0 && cameraIndex<(int)models.size()); + 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() && @@ -675,6 +692,11 @@ std::map MarkerDetector::detect(const cv::Mat & image, float offsetX = cam*subRGBWidth; for(size_t i=0; i(0,0), R.at(0,1), R.at(0,2), tvecs[i].val[0], diff --git a/guilib/src/ui/preferencesDialog.ui b/guilib/src/ui/preferencesDialog.ui index 88411ab5db..d03332e4f4 100644 --- a/guilib/src/ui/preferencesDialog.ui +++ b/guilib/src/ui/preferencesDialog.ui @@ -15780,6 +15780,11 @@ With <0, the length is estimated once for each unique marker, then re-used fo APRILTAG_36h11 + + + ARUCO_MIP_36h12 + +