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

Added Marker/MaxRange and Marker/MinRange parameters (#630)

Browse Source 
* ADD 3 meter limit for marker detections

* ADD Marker/MaxRange and Marker/MinRange parameters for controlling marker detection
ADD ctags ignore

Co-authored-by: John Paul Soliva <soliva@seaos.co.jp>
Co-authored-by: Tim Fronsee <tfronsee21@gmail.com>
This commit is contained in:
David Molina
2020-11-22 06:18:22 +09:00
committed by GitHub
parent 7c4d2bbdf4
commit f467f2af7f
4 changed files with 24 additions and 11 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -50,6 +50,8 @@ private:
cv::Ptr<cv::aruco::DetectorParameters> detectorParams_;
float markerLength_;
float maxDepthError_;
float maxRange_;
float minRange_;
int dictionaryId_;
cv::Ptr<cv::aruco::Dictionary> dictionary_;
#endif

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

@@ -753,6 +753,8 @@ class RTABMAP_EXP Parameters
RTABMAP_PARAM(Marker, VarianceLinear, float, 0.001, "Linear variance to set on marker detections.");
RTABMAP_PARAM(Marker, 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(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, -1, "Maximum range in which markers will be detected. -1 is reserved for unlimited range.")
RTABMAP_PARAM(Marker, MinRange, float, -1, "Miniminum range in which markers will be detected. -1 is reserved for unlimited range.")
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].");
@@ -840,4 +842,3 @@ private:
}
#endif /* PARAMETERS_H_ */

28
corelib/src/MarkerDetector.cpp
View File

@@ -36,6 +36,8 @@ MarkerDetector::MarkerDetector(const ParametersMap & parameters)
#ifdef HAVE_OPENCV_ARUCO
markerLength_ = Parameters::defaultMarkerLength();
maxDepthError_ = Parameters::defaultMarkerMaxDepthError();
maxRange_ = Parameters::defaultMarkerMaxRange();
minRange_ = Parameters::defaultMarkerMinRange();
dictionaryId_ = Parameters::defaultMarkerDictionary();
#if CV_MAJOR_VERSION > 3 || (CV_MAJOR_VERSION == 3 && CV_MINOR_VERSION >=2)
detectorParams_ = cv::aruco::DetectorParameters::create();
@@ -87,6 +89,8 @@ void MarkerDetector::parseParameters(const ParametersMap & parameters)
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)
@@ -191,15 +195,20 @@ std::map<int, Transform> MarkerDetector::detect(const cv::Mat & image, const Cam
}
}
cv::Mat R;
cv::Rodrigues(rvecs[i], R);
Transform t(R.at<double>(0,0), R.at<double>(0,1), R.at<double>(0,2), tvecs[i].val[0],
R.at<double>(1,0), R.at<double>(1,1), R.at<double>(1,2), tvecs[i].val[1],
R.at<double>(2,0), R.at<double>(2,1), R.at<double>(2,2), tvecs[i].val[2]);
Transform pose = model.localTransform() * t;
detections.insert(std::make_pair(ids[i], pose));
UDEBUG("Marker %d detected at %s (%s)", ids[i], pose.prettyPrint().c_str(), t.prettyPrint().c_str());
// Limit the detection range to be between the min / max range.
// If the ranges are -1, allow any detection within that direction.
if((maxRange_ == -1 || tvecs[i].val[2] < maxRange_) &&
(minRange_ == -1 || tvecs[i].val[2] > minRange_))
{
cv::Mat R;
cv::Rodrigues(rvecs[i], R);
Transform t(R.at<double>(0,0), R.at<double>(0,1), R.at<double>(0,2), tvecs[i].val[0],
R.at<double>(1,0), R.at<double>(1,1), R.at<double>(1,2), tvecs[i].val[1],
R.at<double>(2,0), R.at<double>(2,1), R.at<double>(2,2), tvecs[i].val[2]);
Transform pose = model.localTransform() * t;
detections.insert(std::make_pair(ids[i], pose));
UDEBUG("Marker %d detected at %s (%s)", ids[i], pose.prettyPrint().c_str(), t.prettyPrint().c_str());
}
}
if(markerLength_ == 0)
{
@@ -261,4 +270,3 @@ std::map<int, Transform> MarkerDetector::detect(const cv::Mat & image, const Cam
} /* namespace rtabmap */