From 530da0814559d8f165aa767b011d434eb33d7ac8 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Fri, 1 Mar 2019 18:34:49 -0500 Subject: [PATCH] Added Aruco/MaxDepthError parameter --- corelib/include/rtabmap/core/MarkerDetector.h | 1 + corelib/include/rtabmap/core/Parameters.h | 1 + corelib/src/MarkerDetector.cpp | 13 +- guilib/src/PreferencesDialog.cpp | 1 + guilib/src/ui/preferencesDialog.ui | 137 ++++++++---------- 5 files changed, 73 insertions(+), 80 deletions(-) diff --git a/corelib/include/rtabmap/core/MarkerDetector.h b/corelib/include/rtabmap/core/MarkerDetector.h index 259931db..e60eb9ae 100644 --- a/corelib/include/rtabmap/core/MarkerDetector.h +++ b/corelib/include/rtabmap/core/MarkerDetector.h @@ -49,6 +49,7 @@ private: #ifdef HAVE_OPENCV_ARUCO cv::Ptr detectorParams_; float markerLength_; + float maxDepthError_; int dictionaryId_; cv::Ptr dictionary_; #endif diff --git a/corelib/include/rtabmap/core/Parameters.h b/corelib/include/rtabmap/core/Parameters.h index 1f94f1c7..536f6065 100644 --- a/corelib/include/rtabmap/core/Parameters.h +++ b/corelib/include/rtabmap/core/Parameters.h @@ -712,6 +712,7 @@ class RTABMAP_EXP Parameters RTABMAP_PARAM(Aruco, Dictionary, int, 0, "Dictionary to use: DICT_4X4_50=0, DICT_4X4_100=1, DICT_4X4_250=2, DICT_4X4_1000=3, DICT_5X5_50=4, DICT_5X5_100=5, DICT_5X5_250=6, DICT_5X5_1000=7, DICT_6X6_50=8, DICT_6X6_100=9, DICT_6X6_250=10, DICT_6X6_1000=11, DICT_7X7_50=12, DICT_7X7_100=13, DICT_7X7_250=14, DICT_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(Aruco, MarkerLength, 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(Aruco, 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.", kArucoMarkerLength().c_str())); RTABMAP_PARAM(Aruco, VarianceLinear, float, 0.001, "Linear variance to set on marker detections."); RTABMAP_PARAM(Aruco, VarianceAngular, float, 0.01, "Angular variance to set on marker detections. Set to >=9999 to use only position (xyz) constraint in graph optimization."); RTABMAP_PARAM(Aruco, CornerRefinementMethod, int, 0, "Corner refinement method (0: None, 1: Subpixel, 2:contour, 3: AprilTag 2). For OpenCV <3.3.0, this is \"doCornerRefinement\" parameter: set 0 for false and 1 for true."); diff --git a/corelib/src/MarkerDetector.cpp b/corelib/src/MarkerDetector.cpp index 18fcda1f..6caba092 100644 --- a/corelib/src/MarkerDetector.cpp +++ b/corelib/src/MarkerDetector.cpp @@ -35,6 +35,7 @@ MarkerDetector::MarkerDetector(const ParametersMap & parameters) { #ifdef HAVE_OPENCV_ARUCO markerLength_ = Parameters::defaultArucoMarkerLength(); + maxDepthError_ = Parameters::defaultArucoMaxDepthError(); dictionaryId_ = Parameters::defaultArucoDictionary(); #if CV_MAJOR_VERSION > 3 || (CV_MAJOR_VERSION == 3 && CV_MINOR_VERSION >=2) detectorParams_ = cv::aruco::DetectorParameters::create(); @@ -85,6 +86,7 @@ void MarkerDetector::parseParameters(const ParametersMap & parameters) detectorParams_->errorCorrectionRate = 0.6; Parameters::parse(parameters, Parameters::kArucoMarkerLength(), markerLength_); + Parameters::parse(parameters, Parameters::kArucoMaxDepthError(), maxDepthError_); Parameters::parse(parameters, Parameters::kArucoDictionary(), 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) @@ -152,9 +154,9 @@ std::map MarkerDetector::detect(const cv::Mat & image, const Cam // best depth estimation) if(d1>0 && d2>0 && d3>0 && d4>0) { - if( fabs(d1-d2) < 0.01f && - fabs(d1-d3) < 0.01f && - fabs(d1-d4) < 0.01f) + if( fabs(d1-d2) < maxDepthError_ && + fabs(d1-d3) < maxDepthError_ && + fabs(d1-d4) < maxDepthError_) { float depth = (d1+d2+d3+d4)/4.0f; scales.push_back(depth/tvecs[i].val[2]); @@ -165,10 +167,11 @@ std::map MarkerDetector::detect(const cv::Mat & image, const Cam { UWARN("The four marker's corners should be " "perpendicular to camera to estimate correctly " - "the marker's length. Errors: %f, %f, %f > 0.01m." + "the marker's length. Errors: %f, %f, %f > %fm (%s). Four corners: %f %f %f %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), + fabs(d1-d2), fabs(d1-d3), fabs(d1-d4), maxDepthError_, Parameters::kArucoMaxDepthError().c_str(), + d1, d2, d3, d4, Parameters::kArucoMarkerLength().c_str()); detections.clear(); return detections; diff --git a/guilib/src/PreferencesDialog.cpp b/guilib/src/PreferencesDialog.cpp index 7cd991dd..337b5634 100644 --- a/guilib/src/PreferencesDialog.cpp +++ b/guilib/src/PreferencesDialog.cpp @@ -1219,6 +1219,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) : // Aruco marker _ui->ArucoDictionary->setObjectName(Parameters::kArucoDictionary().c_str()); _ui->ArucoMarkerLength->setObjectName(Parameters::kArucoMarkerLength().c_str()); + _ui->ArucoMaxDepthError->setObjectName(Parameters::kArucoMaxDepthError().c_str()); _ui->ArucoVarianceLinear->setObjectName(Parameters::kArucoVarianceLinear().c_str()); _ui->ArucoVarianceAngular->setObjectName(Parameters::kArucoVarianceAngular().c_str()); _ui->ArucoCornerRefinementMethod->setObjectName(Parameters::kArucoCornerRefinementMethod().c_str()); diff --git a/guilib/src/ui/preferencesDialog.ui b/guilib/src/ui/preferencesDialog.ui index a9f6304f..1b98ec6e 100644 --- a/guilib/src/ui/preferencesDialog.ui +++ b/guilib/src/ui/preferencesDialog.ui @@ -95,24 +95,15 @@ 0 0 - 680 - 3008 + 673 + 3038 0 - - 0 - - - 0 - - - 0 - - + 0 @@ -5173,16 +5164,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki Directory of images (optional settings) - - 0 - - - 0 - - - 0 - - + 0 @@ -11139,27 +11121,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - - - Angular variance to set on marker detections. Set to >=9999 to use only position (xyz) constraint in graph optimization. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - - - - - + @@ -11191,7 +11153,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - + Linear variance to set on marker detections. @@ -11217,7 +11179,27 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - + + + + Angular variance to set on marker detections. Set to >=9999 to use only position (xyz) constraint in graph optimization. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + + + + @@ -11258,6 +11240,38 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag + + + + 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 + + + + + + + m + + + 4 + + + 0.000100000000000 + + + 0.010000000000000 + + + 0.100000000000000 + + + @@ -15819,16 +15833,7 @@ Lower the ratio -> higher the precision. 0 - - 0 - - - 0 - - - 0 - - + 0 @@ -15908,16 +15913,7 @@ Lower the ratio -> higher the precision. 0 - - 0 - - - 0 - - - 0 - - + 0 @@ -16029,16 +16025,7 @@ Lower the ratio -> higher the precision. 0 - - 0 - - - 0 - - - 0 - - + 0