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 > 4 || (CV_MAJOR_VERSION == 4 && CV_MINOR_VERSION >= 7)
43  detectorParams_.reset(new cv::aruco::DetectorParameters());
44 #elif CV_MAJOR_VERSION > 3 || (CV_MAJOR_VERSION == 3 && CV_MINOR_VERSION >=2)
45  detectorParams_ = cv::aruco::DetectorParameters::create();
46 #else
47  detectorParams_.reset(new cv::aruco::DetectorParameters());
48 #endif
49 #if CV_MAJOR_VERSION > 4 || (CV_MAJOR_VERSION == 4 && CV_MINOR_VERSION >= 7)
50  detectorParams_->cornerRefinementMethod = (cv::aruco::CornerRefineMethod) Parameters::defaultMarkerCornerRefinementMethod();
51 #elif CV_MAJOR_VERSION > 3 || (CV_MAJOR_VERSION == 3 && CV_MINOR_VERSION >=3)
52  detectorParams_->cornerRefinementMethod = Parameters::defaultMarkerCornerRefinementMethod();
53 #else
54  detectorParams_->doCornerRefinement = Parameters::defaultMarkerCornerRefinementMethod()!=0;
55 #endif
56  parseParameters(parameters);
57 #endif
58 }
59 
60 MarkerDetector::~MarkerDetector() {
61 
62 }
63 
64 void MarkerDetector::parseParameters(const ParametersMap & parameters)
65 {
66 #ifdef HAVE_OPENCV_ARUCO
67  detectorParams_->adaptiveThreshWinSizeMin = 3;
68  detectorParams_->adaptiveThreshWinSizeMax = 23;
69  detectorParams_->adaptiveThreshWinSizeStep = 10;
70  detectorParams_->adaptiveThreshConstant = 7;
71  detectorParams_->minMarkerPerimeterRate = 0.03;
72  detectorParams_->maxMarkerPerimeterRate = 4.0;
73  detectorParams_->polygonalApproxAccuracyRate = 0.03;
74  detectorParams_->minCornerDistanceRate = 0.05;
75  detectorParams_->minDistanceToBorder = 3;
76  detectorParams_->minMarkerDistanceRate = 0.05;
77 #if CV_MAJOR_VERSION > 4 || (CV_MAJOR_VERSION == 4 && CV_MINOR_VERSION >= 7)
78  int cornerRefinementMethod;
79  Parameters::parse(parameters, Parameters::kMarkerCornerRefinementMethod(), cornerRefinementMethod);
80  detectorParams_->cornerRefinementMethod = (cv::aruco::CornerRefineMethod)cornerRefinementMethod;
81 #elif CV_MAJOR_VERSION > 3 || (CV_MAJOR_VERSION == 3 && CV_MINOR_VERSION >=3)
82  Parameters::parse(parameters, Parameters::kMarkerCornerRefinementMethod(), detectorParams_->cornerRefinementMethod);
83 #else
84  int doCornerRefinement = detectorParams_->doCornerRefinement?1:0;
85  Parameters::parse(parameters, Parameters::kMarkerCornerRefinementMethod(), doCornerRefinement);
86  detectorParams_->doCornerRefinement = doCornerRefinement!=0;
87 #endif
88  detectorParams_->cornerRefinementWinSize = 5;
89  detectorParams_->cornerRefinementMaxIterations = 30;
90  detectorParams_->cornerRefinementMinAccuracy = 0.1;
91  detectorParams_->markerBorderBits = 1;
92  detectorParams_->perspectiveRemovePixelPerCell = 4;
93  detectorParams_->perspectiveRemoveIgnoredMarginPerCell = 0.13;
94  detectorParams_->maxErroneousBitsInBorderRate = 0.35;
95  detectorParams_->minOtsuStdDev = 5.0;
96  detectorParams_->errorCorrectionRate = 0.6;
97 
98  Parameters::parse(parameters, Parameters::kMarkerLength(), markerLength_);
99  Parameters::parse(parameters, Parameters::kMarkerMaxDepthError(), maxDepthError_);
100  Parameters::parse(parameters, Parameters::kMarkerMaxRange(), maxRange_);
101  Parameters::parse(parameters, Parameters::kMarkerMinRange(), minRange_);
102  Parameters::parse(parameters, Parameters::kMarkerDictionary(), dictionaryId_);
103 #if CV_MAJOR_VERSION < 3 || (CV_MAJOR_VERSION == 3 && (CV_MINOR_VERSION <4 || (CV_MINOR_VERSION ==4 && CV_SUBMINOR_VERSION<2)))
104  if(dictionaryId_ >= 17)
105  {
106  UERROR("Cannot set AprilTag dictionary. OpenCV version should be at least 3.4.2, "
107  "current version is %s. Setting %s to default (%d)",
108  CV_VERSION,
109  Parameters::kMarkerDictionary().c_str(),
110  Parameters::defaultMarkerDictionary());
111  dictionaryId_ = Parameters::defaultMarkerDictionary();
112  }
113 #endif
114 #if CV_MAJOR_VERSION > 4 || (CV_MAJOR_VERSION == 4 && CV_MINOR_VERSION >= 7)
115  dictionary_.reset(new cv::aruco::Dictionary());
116  *dictionary_ = cv::aruco::getPredefinedDictionary(cv::aruco::PredefinedDictionaryType(dictionaryId_));
117 #elif CV_MAJOR_VERSION > 3 || (CV_MAJOR_VERSION == 3 && CV_MINOR_VERSION >=2)
118  dictionary_ = cv::aruco::getPredefinedDictionary(cv::aruco::PREDEFINED_DICTIONARY_NAME(dictionaryId_));
119 #else
120  dictionary_.reset(new cv::aruco::Dictionary());
121  *dictionary_ = cv::aruco::getPredefinedDictionary(cv::aruco::PREDEFINED_DICTIONARY_NAME(dictionaryId_));
122 #endif
123 #endif
124 }
125 
126 std::map<int, Transform> MarkerDetector::detect(const cv::Mat & image, const CameraModel & model, const cv::Mat & depth, float * markerLengthOut, cv::Mat * imageWithDetections)
127 {
128  std::map<int, Transform> detections;
129  std::map<int, MarkerInfo> infos = detect(image, model, depth, std::map<int, float>(), imageWithDetections);
130 
131  for(std::map<int, MarkerInfo>::iterator iter=infos.begin(); iter!=infos.end(); ++iter)
132  {
133  detections.insert(std::make_pair(iter->first, iter->second.pose()));
134 
135  if(markerLengthOut)
136  {
137  *markerLengthOut = iter->second.length();
138  }
139  }
140 
141  return detections;
142 }
143 
144 std::map<int, MarkerInfo> MarkerDetector::detect(const cv::Mat & image,
145  const std::vector<CameraModel> & models,
146  const cv::Mat & depth,
147  const std::map<int, float> & markerLengths,
148  cv::Mat * imageWithDetections)
149 {
150  UASSERT(!models.empty() && !image.empty());
151  UASSERT(int((image.cols/models.size())*models.size()) == image.cols);
152  UASSERT(int((depth.cols/models.size())*models.size()) == depth.cols);
153  int subRGBWidth = image.cols/models.size();
154  int subDepthWidth = depth.cols/models.size();
155 
156  std::map<int, MarkerInfo> allInfo;
157  for(size_t i=0; i<models.size(); ++i)
158  {
159  cv::Mat subImage(image, cv::Rect(subRGBWidth*i, 0, subRGBWidth, image.rows));
160  cv::Mat subDepth;
161  if(!depth.empty())
162  subDepth = cv::Mat(depth, cv::Rect(subDepthWidth*i, 0, subDepthWidth, depth.rows));
163  CameraModel model = models[i];
164  cv::Mat subImageWithDetections;
165  std::map<int, MarkerInfo> subInfo = detect(subImage, model, subDepth, markerLengths, imageWithDetections?&subImageWithDetections:0);
166  if(ULogger::level() >= ULogger::kWarning)
167  {
168  for(std::map<int, MarkerInfo>::iterator iter=subInfo.begin(); iter!=subInfo.end(); ++iter)
169  {
170  std::pair<std::map<int, MarkerInfo>::iterator, bool> inserted = allInfo.insert(*iter);
171  if(!inserted.second)
172  {
173  UWARN("Marker %d already added by another camera, ignoring detection from camera %d", iter->first, i);
174  }
175  }
176  }
177  else
178  {
179  allInfo.insert(subInfo.begin(), subInfo.end());
180  }
181  if(imageWithDetections)
182  {
183  if(i==0)
184  {
185  *imageWithDetections = image.clone();
186  }
187  if(!subImageWithDetections.empty())
188  {
189  subImageWithDetections.copyTo(cv::Mat(*imageWithDetections, cv::Rect(subRGBWidth*i, 0, subRGBWidth, image.rows)));
190  }
191  }
192  }
193  return allInfo;
194 }
195 
196 std::map<int, MarkerInfo> MarkerDetector::detect(const cv::Mat & image,
197  const CameraModel & model,
198  const cv::Mat & depth,
199  const std::map<int, float> & markerLengths,
200  cv::Mat * imageWithDetections)
201 {
202  if(!image.empty() && image.cols != model.imageWidth())
203  {
204  UERROR("This method cannot handle multi-camera marker detection, use the other function version supporting it.");
205  return std::map<int, MarkerInfo>();
206  }
207 
208  std::map<int, MarkerInfo> detections;
209 
210 #ifdef HAVE_OPENCV_ARUCO
211 
212  std::vector< int > ids;
213  std::vector< std::vector< cv::Point2f > > corners, rejected;
214  std::vector< cv::Vec3d > rvecs, tvecs;
215 
216  // detect markers and estimate pose
217 #if CV_MAJOR_VERSION > 3 || (CV_MAJOR_VERSION == 3 && CV_MINOR_VERSION >=2)
218  cv::aruco::detectMarkers(image, dictionary_, corners, ids, detectorParams_, rejected);
219 #else
220  cv::aruco::detectMarkers(image, *dictionary_, corners, ids, *detectorParams_, rejected);
221 #endif
222  UDEBUG("Markers detected=%d rejected=%d", (int)ids.size(), (int)rejected.size());
223  if(ids.size() > 0)
224  {
225  float rgbToDepthFactorX = 1.0f;
226  float rgbToDepthFactorY = 1.0f;
227  if(!depth.empty())
228  {
229  rgbToDepthFactorX = 1.0f/(model.imageWidth()>0?model.imageWidth()/depth.cols:1);
230  rgbToDepthFactorY = 1.0f/(model.imageHeight()>0?model.imageHeight()/depth.rows:1);
231  }
232  else if(markerLength_ == 0)
233  {
234  if(depth.empty())
235  {
236  UERROR("Depth image is empty, please set %s parameter to non-null.", Parameters::kMarkerLength().c_str());
237  return detections;
238  }
239  }
240 
241  cv::aruco::estimatePoseSingleMarkers(corners, markerLength_<=0.0?1.0f:markerLength_, model.K(), model.D(), rvecs, tvecs);
242  std::vector<float> scales;
243  for(size_t i=0; i<ids.size(); ++i)
244  {
245  float length = 0.0f;
246  std::map<int, float>::const_iterator findIter = markerLengths.find(ids[i]);
247  if(!depth.empty() && (markerLength_ == 0 || (markerLength_<0 && findIter==markerLengths.end())))
248  {
249  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);
250  float d1 = util2d::getDepth(depth, corners[i][0].x*rgbToDepthFactorX, corners[i][0].y*rgbToDepthFactorY, true, 0.02f, true);
251  float d2 = util2d::getDepth(depth, corners[i][1].x*rgbToDepthFactorX, corners[i][1].y*rgbToDepthFactorY, true, 0.02f, true);
252  float d3 = util2d::getDepth(depth, corners[i][2].x*rgbToDepthFactorX, corners[i][2].y*rgbToDepthFactorY, true, 0.02f, true);
253  float d4 = util2d::getDepth(depth, corners[i][3].x*rgbToDepthFactorX, corners[i][3].y*rgbToDepthFactorY, true, 0.02f, true);
254  // Accept measurement only if all 4 depth values are valid and
255  // they are at the same depth (camera should be perpendicular to marker for
256  // best depth estimation)
257  if(d>0 && d1>0 && d2>0 && d3>0 && d4>0)
258  {
259  float scale = d/tvecs[i].val[2];
260 
261  if( fabs(d-d1) < maxDepthError_ &&
262  fabs(d-d2) < maxDepthError_ &&
263  fabs(d-d3) < maxDepthError_ &&
264  fabs(d-d4) < maxDepthError_)
265  {
266  length = scale;
267  scales.push_back(length);
268  tvecs[i] *= scales.back();
269  UWARN("Automatic marker length estimation: id=%d depth=%fm length=%fm", ids[i], d, scales.back());
270  }
271  else
272  {
273  UWARN("The four marker's corners should be "
274  "perpendicular to camera to estimate correctly "
275  "the marker's length. Errors: %f, %f, %f > %fm (%s). Four corners: %f %f %f %f (middle=%f). "
276  "Parameter %s can be set to non-null to skip automatic "
277  "marker length estimation. Detections are ignored.",
278  fabs(d1-d2), fabs(d1-d3), fabs(d1-d4), maxDepthError_, Parameters::kMarkerMaxDepthError().c_str(),
279  d1, d2, d3, d4, d,
280  Parameters::kMarkerLength().c_str());
281  continue;
282  }
283  }
284  else
285  {
286  UWARN("Some depth values (%f,%f,%f,%f, middle=%f) cannot be detected on the "
287  "marker's corners, cannot initialize marker length. "
288  "Parameter %s can be set to non-null to skip automatic "
289  "marker length estimation. Detections are ignored.",
290  d1,d2,d3,d4,d,
291  Parameters::kMarkerLength().c_str());
292  continue;
293  }
294  }
295  else if(markerLength_ < 0)
296  {
297  if(findIter!=markerLengths.end())
298  {
299  length = findIter->second;
300  tvecs[i] *= length;
301  }
302  else
303  {
304  UWARN("Cannot find marker length for marker %d, ignoring this marker (count=%d)", ids[i], (int)markerLengths.size());
305  continue;
306  }
307  }
308  else if(markerLength_ > 0)
309  {
310  length = markerLength_;
311  }
312  else
313  {
314  continue;
315  }
316 
317  // Limit the detection range to be between the min / max range.
318  // If the ranges are -1, allow any detection within that direction.
319  if((maxRange_ <= 0 || tvecs[i].val[2] < maxRange_) &&
320  (minRange_ <= 0 || tvecs[i].val[2] > minRange_))
321  {
322  cv::Mat R;
323  cv::Rodrigues(rvecs[i], R);
324  Transform t(R.at<double>(0,0), R.at<double>(0,1), R.at<double>(0,2), tvecs[i].val[0],
325  R.at<double>(1,0), R.at<double>(1,1), R.at<double>(1,2), tvecs[i].val[1],
326  R.at<double>(2,0), R.at<double>(2,1), R.at<double>(2,2), tvecs[i].val[2]);
327  Transform pose = model.localTransform() * t;
328  detections.insert(std::make_pair(ids[i], MarkerInfo(ids[i], length, pose)));
329  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());
330  }
331  }
332  if(markerLength_ == 0 && !scales.empty())
333  {
334  float sum = 0.0f;
335  float maxError = 0.0f;
336  for(size_t i=0; i<scales.size(); ++i)
337  {
338  if(i>0)
339  {
340  float error = fabs(scales[i]-scales[0]);
341  if(error > 0.001f)
342  {
343  UWARN("The marker's length detected between 2 of the "
344  "markers doesn't match (%fm vs %fm)."
345  "Parameter %s can be set to non-null to skip automatic "
346  "marker length estimation. Detections are ignored.",
347  scales[i], scales[0],
348  Parameters::kMarkerLength().c_str());
349  detections.clear();
350  return detections;
351  }
352  if(error > maxError)
353  {
354  maxError = error;
355  }
356  }
357  sum += scales[i];
358  }
359  markerLength_ = sum/float(scales.size());
360  UWARN("Final marker length estimated = %fm, max error=%fm (used for subsequent detections)", markerLength_, maxError);
361  }
362  }
363 
364  if(imageWithDetections)
365  {
366  image.copyTo(*imageWithDetections);
367  if(!ids.empty())
368  {
369  cv::aruco::drawDetectedMarkers(*imageWithDetections, corners, ids);
370 
371  for(unsigned int i = 0; i < ids.size(); i++)
372  {
373  std::map<int, MarkerInfo>::iterator iter = detections.find(ids[i]);
374  if(iter!=detections.end())
375  {
376 #if CV_MAJOR_VERSION > 4 || (CV_MAJOR_VERSION == 4 && (CV_MINOR_VERSION >1 || (CV_MINOR_VERSION==1 && CV_PATCH_VERSION>=1)))
377  cv::drawFrameAxes(*imageWithDetections, model.K(), model.D(), rvecs[i], tvecs[i], iter->second.length() * 0.5f);
378 #else
379  cv::aruco::drawAxis(*imageWithDetections, model.K(), model.D(), rvecs[i], tvecs[i], iter->second.length() * 0.5f);
380 #endif
381  }
382  }
383  }
384  }
385 
386 #else
387  UERROR("RTAB-Map is not built with \"aruco\" module from OpenCV.");
388 #endif
389 
390  return detections;
391 }
392 
393 
394 } /* namespace rtabmap */
d
d
rtabmap::Parameters::parse
static bool parse(const ParametersMap &parameters, const std::string &key, bool &value)
Definition: Parameters.cpp:497
rtabmap::CameraModel::imageHeight
int imageHeight() const
Definition: CameraModel.h:121
f
f
x
x
rtabmap::MarkerDetector::detect
std::map< int, MarkerInfo > detect(const cv::Mat &image, const std::vector< CameraModel > &models, const cv::Mat &depth=cv::Mat(), const std::map< int, float > &markerLengths=std::map< int, float >(), cv::Mat *imageWithDetections=0)
Definition: MarkerDetector.cpp:144
glm::scale
GLM_FUNC_DECL detail::tmat4x4< T, P > scale(detail::tmat4x4< T, P > const &m, detail::tvec3< T, P > const &v)
rtabmap::ParametersMap
std::map< std::string, std::string > ParametersMap
Definition: Parameters.h:43
UASSERT
#define UASSERT(condition)
rtabmap::CameraModel::K
cv::Mat K() const
Definition: CameraModel.h:110
rtabmap::Transform::prettyPrint
std::string prettyPrint() const
Definition: Transform.cpp:316
rtabmap::CameraModel::localTransform
const Transform & localTransform() const
Definition: CameraModel.h:116
ULogger::level
static ULogger::Level level()
Definition: ULogger.h:340
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:64
trace.model
model
Definition: trace.py:4
rtabmap::CameraModel::imageWidth
int imageWidth() const
Definition: CameraModel.h:120
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
glm::length
GLM_FUNC_DECL genType::value_type length(genType const &x)
rtabmap::d1
const int d1
Definition: SuperPoint.cc:20
rtabmap::CameraModel::D
cv::Mat D() const
Definition: CameraModel.h:111

rtabmap
Author(s): Mathieu Labbe
autogenerated on Mon Jan 23 2023 03:37:29