MarkerDetector.cpp
Go to the documentation of this file.
1 /*
2 Copyright (c) 2010-2019, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
3 All rights reserved.
4 
5 Redistribution and use in source and binary forms, with or without
6 modification, are permitted provided that the following conditions are met:
7  * Redistributions of source code must retain the above copyright
8  notice, this list of conditions and the following disclaimer.
9  * Redistributions in binary form must reproduce the above copyright
10  notice, this list of conditions and the following disclaimer in the
11  documentation and/or other materials provided with the distribution.
12  * Neither the name of the Universite de Sherbrooke nor the
13  names of its contributors may be used to endorse or promote products
14  derived from this software without specific prior written permission.
15 
16 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
17 ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
18 WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
19 DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
20 DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
21 (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
22 LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
23 ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
24 (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
25 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
26 */
27 
28 #include <rtabmap/core/MarkerDetector.h>
29 #include <rtabmap/core/util2d.h>
30 #include <rtabmap/utilite/ULogger.h>
31 
32 namespace rtabmap {
33 
34 MarkerDetector::MarkerDetector(const ParametersMap & parameters)
35 {
36 #ifdef HAVE_OPENCV_ARUCO
37  markerLength_ = Parameters::defaultMarkerLength();
38  maxDepthError_ = Parameters::defaultMarkerMaxDepthError();
39  maxRange_ = Parameters::defaultMarkerMaxRange();
40  minRange_ = Parameters::defaultMarkerMinRange();
41  dictionaryId_ = Parameters::defaultMarkerDictionary();
42 #if CV_MAJOR_VERSION > 3 || (CV_MAJOR_VERSION == 3 && CV_MINOR_VERSION >=2)
43  detectorParams_ = cv::aruco::DetectorParameters::create();
44 #else
45  detectorParams_.reset(new cv::aruco::DetectorParameters());
46 #endif
47 #if CV_MAJOR_VERSION > 3 || (CV_MAJOR_VERSION == 3 && CV_MINOR_VERSION >=3)
48  detectorParams_->cornerRefinementMethod = Parameters::defaultMarkerCornerRefinementMethod();
49 #else
50  detectorParams_->doCornerRefinement = Parameters::defaultMarkerCornerRefinementMethod()!=0;
51 #endif
52  parseParameters(parameters);
53 #endif
54 }
55 
56 MarkerDetector::~MarkerDetector() {
57 
58 }
59 
60 void MarkerDetector::parseParameters(const ParametersMap & parameters)
61 {
62 #ifdef HAVE_OPENCV_ARUCO
63  detectorParams_->adaptiveThreshWinSizeMin = 3;
64  detectorParams_->adaptiveThreshWinSizeMax = 23;
65  detectorParams_->adaptiveThreshWinSizeStep = 10;
66  detectorParams_->adaptiveThreshConstant = 7;
67  detectorParams_->minMarkerPerimeterRate = 0.03;
68  detectorParams_->maxMarkerPerimeterRate = 4.0;
69  detectorParams_->polygonalApproxAccuracyRate = 0.03;
70  detectorParams_->minCornerDistanceRate = 0.05;
71  detectorParams_->minDistanceToBorder = 3;
72  detectorParams_->minMarkerDistanceRate = 0.05;
73 #if CV_MAJOR_VERSION > 3 || (CV_MAJOR_VERSION == 3 && CV_MINOR_VERSION >=3)
74  Parameters::parse(parameters, Parameters::kMarkerCornerRefinementMethod(), detectorParams_->cornerRefinementMethod);
75 #else
76  int doCornerRefinement = detectorParams_->doCornerRefinement?1:0;
77  Parameters::parse(parameters, Parameters::kMarkerCornerRefinementMethod(), doCornerRefinement);
78  detectorParams_->doCornerRefinement = doCornerRefinement!=0;
79 #endif
80  detectorParams_->cornerRefinementWinSize = 5;
81  detectorParams_->cornerRefinementMaxIterations = 30;
82  detectorParams_->cornerRefinementMinAccuracy = 0.1;
83  detectorParams_->markerBorderBits = 1;
84  detectorParams_->perspectiveRemovePixelPerCell = 4;
85  detectorParams_->perspectiveRemoveIgnoredMarginPerCell = 0.13;
86  detectorParams_->maxErroneousBitsInBorderRate = 0.35;
87  detectorParams_->minOtsuStdDev = 5.0;
88  detectorParams_->errorCorrectionRate = 0.6;
89 
90  Parameters::parse(parameters, Parameters::kMarkerLength(), markerLength_);
91  Parameters::parse(parameters, Parameters::kMarkerMaxDepthError(), maxDepthError_);
92  Parameters::parse(parameters, Parameters::kMarkerMaxRange(), maxRange_);
93  Parameters::parse(parameters, Parameters::kMarkerMinRange(), minRange_);
94  Parameters::parse(parameters, Parameters::kMarkerDictionary(), dictionaryId_);
95 #if CV_MAJOR_VERSION < 3 || (CV_MAJOR_VERSION == 3 && (CV_MINOR_VERSION <4 || (CV_MINOR_VERSION ==4 && CV_SUBMINOR_VERSION<2)))
96  if(dictionaryId_ >= 17)
97  {
98  UERROR("Cannot set AprilTag dictionary. OpenCV version should be at least 3.4.2, "
99  "current version is %s. Setting %s to default (%d)",
100  CV_VERSION,
101  Parameters::kMarkerDictionary().c_str(),
102  Parameters::defaultMarkerDictionary());
103  dictionaryId_ = Parameters::defaultMarkerDictionary();
104  }
105 #endif
106 #if CV_MAJOR_VERSION > 3 || (CV_MAJOR_VERSION == 3 && CV_MINOR_VERSION >=2)
107  dictionary_ = cv::aruco::getPredefinedDictionary(cv::aruco::PREDEFINED_DICTIONARY_NAME(dictionaryId_));
108 #else
109  dictionary_.reset(new cv::aruco::Dictionary());
110  *dictionary_ = cv::aruco::getPredefinedDictionary(cv::aruco::PREDEFINED_DICTIONARY_NAME(dictionaryId_));
111 #endif
112 #endif
113 }
114 
115 std::map<int, Transform> MarkerDetector::detect(const cv::Mat & image, const CameraModel & model, const cv::Mat & depth, float * markerLengthOut, cv::Mat * imageWithDetections)
116 {
117  std::map<int, Transform> detections;
118 
119 #ifdef HAVE_OPENCV_ARUCO
120 
121  std::vector< int > ids;
122  std::vector< std::vector< cv::Point2f > > corners, rejected;
123  std::vector< cv::Vec3d > rvecs, tvecs;
124 
125  // detect markers and estimate pose
126 #if CV_MAJOR_VERSION > 3 || (CV_MAJOR_VERSION == 3 && CV_MINOR_VERSION >=2)
127  cv::aruco::detectMarkers(image, dictionary_, corners, ids, detectorParams_, rejected);
128 #else
129  cv::aruco::detectMarkers(image, *dictionary_, corners, ids, *detectorParams_, rejected);
130 #endif
131  UDEBUG("Markers detected=%d rejected=%d", (int)ids.size(), (int)rejected.size());
132  if(ids.size() > 0)
133  {
134  float rgbToDepthFactorX = 1.0f;
135  float rgbToDepthFactorY = 1.0f;
136  if(markerLength_ == 0)
137  {
138  if(depth.empty())
139  {
140  UERROR("Depth image is empty, please set %s parameter to non-null.", Parameters::kMarkerLength().c_str());
141  return detections;
142  }
143  rgbToDepthFactorX = 1.0f/(model.imageWidth()>0?model.imageWidth()/depth.cols:1);
144  rgbToDepthFactorY = 1.0f/(model.imageHeight()>0?model.imageHeight()/depth.rows:1);
145  }
146 
147  cv::aruco::estimatePoseSingleMarkers(corners, markerLength_==0?1.0f:markerLength_, model.K(), model.D(), rvecs, tvecs);
148  std::vector<float> scales;
149  for(size_t i=0; i<ids.size(); ++i)
150  {
151  if(markerLength_ == 0)
152  {
153  float d1 = util2d::getDepth(depth, corners[i][0].x*rgbToDepthFactorX, corners[i][0].y*rgbToDepthFactorY, true, 0.02f, true);
154  float d2 = util2d::getDepth(depth, corners[i][1].x*rgbToDepthFactorX, corners[i][1].y*rgbToDepthFactorY, true, 0.02f, true);
155  float d3 = util2d::getDepth(depth, corners[i][2].x*rgbToDepthFactorX, corners[i][2].y*rgbToDepthFactorY, true, 0.02f, true);
156  float d4 = util2d::getDepth(depth, corners[i][3].x*rgbToDepthFactorX, corners[i][3].y*rgbToDepthFactorY, true, 0.02f, true);
157  // Accept measurement only if all 4 depth values are valid and
158  // they are at the same depth (camera should be perpendicular to marker for
159  // best depth estimation)
160  if(d1>0 && d2>0 && d3>0 && d4>0)
161  {
162  if( fabs(d1-d2) < maxDepthError_ &&
163  fabs(d1-d3) < maxDepthError_ &&
164  fabs(d1-d4) < maxDepthError_)
165  {
166  float depth = (d1+d2+d3+d4)/4.0f;
167  scales.push_back(depth/tvecs[i].val[2]);
168  tvecs[i] *= scales.back();
169  UWARN("Automatic marker length estimation: id=%d depth=%fm length=%fm", ids[i], depth, scales.back());
170  }
171  else
172  {
173  UWARN("The four marker's corners should be "
174  "perpendicular to camera to estimate correctly "
175  "the marker's length. Errors: %f, %f, %f > %fm (%s). Four corners: %f %f %f %f. "
176  "Parameter %s can be set to non-null to skip automatic "
177  "marker length estimation. Detections are ignored.",
178  fabs(d1-d2), fabs(d1-d3), fabs(d1-d4), maxDepthError_, Parameters::kMarkerMaxDepthError().c_str(),
179  d1, d2, d3, d4,
180  Parameters::kMarkerLength().c_str());
181  detections.clear();
182  return detections;
183  }
184  }
185  else
186  {
187  UWARN("Some depth values (%f,%f,%f,%f) cannot be detected on the "
188  "marker's corners, cannot initialize marker length. "
189  "Parameter %s can be set to non-null to skip automatic "
190  "marker length estimation. Detections are ignored.",
191  d1,d2,d3,d4,
192  Parameters::kMarkerLength().c_str());
193  detections.clear();
194  return detections;
195  }
196  }
197 
198  // Limit the detection range to be between the min / max range.
199  // If the ranges are -1, allow any detection within that direction.
200  if((maxRange_ <= 0 || tvecs[i].val[2] < maxRange_) &&
201  (minRange_ <= 0 || tvecs[i].val[2] > minRange_))
202  {
203  cv::Mat R;
204  cv::Rodrigues(rvecs[i], R);
205  Transform t(R.at<double>(0,0), R.at<double>(0,1), R.at<double>(0,2), tvecs[i].val[0],
206  R.at<double>(1,0), R.at<double>(1,1), R.at<double>(1,2), tvecs[i].val[1],
207  R.at<double>(2,0), R.at<double>(2,1), R.at<double>(2,2), tvecs[i].val[2]);
208  Transform pose = model.localTransform() * t;
209  detections.insert(std::make_pair(ids[i], pose));
210  UDEBUG("Marker %d detected at %s (%s)", ids[i], pose.prettyPrint().c_str(), t.prettyPrint().c_str());
211  }
212  }
213  if(markerLength_ == 0)
214  {
215  float sum = 0.0f;
216  float maxError = 0.0f;
217  for(size_t i=0; i<scales.size(); ++i)
218  {
219  if(i>0)
220  {
221  float error = fabs(scales[i]-scales[0]);
222  if(error > 0.001f)
223  {
224  UWARN("The marker's length detected between 2 of the "
225  "markers doesn't match (%d->%fm vs %d->%fm)."
226  "Parameter %s can be set to non-null to skip automatic "
227  "marker length estimation. Detections are ignored.",
228  ids[i], scales[i], ids[0], scales[0],
229  Parameters::kMarkerLength().c_str());
230  detections.clear();
231  return detections;
232  }
233  if(error > maxError)
234  {
235  maxError = error;
236  }
237  }
238  sum += scales[i];
239  }
240  markerLength_ = sum/float(scales.size());
241  UWARN("Final marker length estimated = %fm, max error=%fm (used for subsequent detections)", markerLength_, maxError);
242  }
243  }
244 
245  if(markerLengthOut)
246  {
247  *markerLengthOut = markerLength_;
248  }
249 
250  if(imageWithDetections)
251  {
252  image.copyTo(*imageWithDetections);
253  if(ids.size() > 0)
254  {
255  cv::aruco::drawDetectedMarkers(*imageWithDetections, corners, ids);
256 
257  for(unsigned int i = 0; i < ids.size(); i++)
258  {
259  cv::aruco::drawAxis(*imageWithDetections, model.K(), model.D(), rvecs[i], tvecs[i], markerLength_ * 0.5f);
260  }
261  }
262  }
263 
264 #else
265  UERROR("RTAB-Map is not built with \"aruco\" module from OpenCV.");
266 #endif
267 
268  return detections;
269 }
270 
271 
272 } /* namespace rtabmap */
rtabmap::CameraModel::imageWidth
int imageWidth() const
Definition: CameraModel.h:120
rtabmap::Parameters::parse
static bool parse(const ParametersMap &parameters, const std::string &key, bool &value)
Definition: Parameters.cpp:476
rtabmap::Transform::prettyPrint
std::string prettyPrint() const
Definition: Transform.cpp:295
f
f
rtabmap::ParametersMap
std::map< std::string, std::string > ParametersMap
Definition: Parameters.h:43
rtabmap::CameraModel::D
cv::Mat D() const
Definition: CameraModel.h:111
rtabmap::MarkerDetector::MarkerDetector
MarkerDetector(const ParametersMap &parameters=ParametersMap())
Definition: MarkerDetector.cpp:34
UDEBUG
#define UDEBUG(...)
UERROR
#define UERROR(...)
ULogger.h
ULogger class and convenient macros.
UWARN
#define UWARN(...)
rtabmap::MarkerDetector::parseParameters
void parseParameters(const ParametersMap &parameters)
Definition: MarkerDetector.cpp:60
rtabmap::MarkerDetector::detect
std::map< int, Transform > detect(const cv::Mat &image, const CameraModel &model, const cv::Mat &depth=cv::Mat(), float *estimatedMarkerLength=0, cv::Mat *imageWithDetections=0)
Definition: MarkerDetector.cpp:115
rtabmap::CameraModel::K
cv::Mat K() const
Definition: CameraModel.h:110
rtabmap::util2d::getDepth
float RTABMAP_EXP getDepth(const cv::Mat &depthImage, float x, float y, bool smoothing, float depthErrorRatio=0.02f, bool estWithNeighborsIfNull=false)
Definition: util2d.cpp:947
rtabmap::d1
const int d1
Definition: SuperPoint.cc:20
rtabmap::CameraModel::imageHeight
int imageHeight() const
Definition: CameraModel.h:121
rtabmap::CameraModel::localTransform
const Transform & localTransform() const
Definition: CameraModel.h:116

rtabmap
Author(s): Mathieu Labbe
autogenerated on Mon Dec 14 2020 03:34:59