i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-02 17:40:23 +08:00
Code Issues Packages Projects Releases Wiki Activity

Added Aruco/MaxDepthError parameter

Browse Source
This commit is contained in:
matlabbe
2019-03-01 18:34:49 -05:00
parent c272f9538a
commit 530da08145
5 changed files with 73 additions and 80 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
corelib/include/rtabmap/core/MarkerDetector.h
View File

@@ -49,6 +49,7 @@ private:
#ifdef HAVE_OPENCV_ARUCO
cv::Ptr<cv::aruco::DetectorParameters> detectorParams_;
float markerLength_;
float maxDepthError_;
int dictionaryId_;
cv::Ptr<cv::aruco::Dictionary> dictionary_;
#endif

1
corelib/include/rtabmap/core/Parameters.h
View File

@@ -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.");

13
corelib/src/MarkerDetector.cpp
View File

@@ -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<int, Transform> 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<int, Transform> 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;