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 = cv::Mat(image.size(), subImageWithDetections.type());
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  if(image.channels()==1)
367  {
368  cv::cvtColor(image, *imageWithDetections, cv::COLOR_GRAY2BGR);
369  }
370  else
371  {
372  image.copyTo(*imageWithDetections);
373  }
374  if(!ids.empty())
375  {
376  cv::aruco::drawDetectedMarkers(*imageWithDetections, corners, ids);
377 
378  for(unsigned int i = 0; i < ids.size(); i++)
379  {
380  std::map<int, MarkerInfo>::iterator iter = detections.find(ids[i]);
381  if(iter!=detections.end())
382  {
383 #if CV_MAJOR_VERSION > 4 || (CV_MAJOR_VERSION == 4 && (CV_MINOR_VERSION >1 || (CV_MINOR_VERSION==1 && CV_PATCH_VERSION>=1)))
384  cv::drawFrameAxes(*imageWithDetections, model.K(), model.D(), rvecs[i], tvecs[i], iter->second.length() * 0.5f);
385 #else
386  cv::aruco::drawAxis(*imageWithDetections, model.K(), model.D(), rvecs[i], tvecs[i], iter->second.length() * 0.5f);
387 #endif
388  }
389  }
390  }
391  }
392 
393 #else
394  UERROR("RTAB-Map is not built with \"aruco\" module from OpenCV.");
395 #endif
396 
397  return detections;
398 }
399 
400 
401 } /* namespace rtabmap */
rtabmap::MarkerDetector::detect
RTABMAP_DEPRECATED MapIdPose detect(const cv::Mat &image, const CameraModel &model, const cv::Mat &depth=cv::Mat(), float *estimatedMarkerLength=0, cv::Mat *imageWithDetections=0)
Definition: MarkerDetector.cpp:126
glm::length
GLM_FUNC_DECL genType::value_type length(genType const &x)
rtabmap::d1
const int d1
Definition: SuperPoint.cc:20
rtabmap::CameraModel
Definition: CameraModel.h:38
rtabmap::MarkerInfo
Definition: MarkerDetector.h:43
rtabmap::MarkerDetector::parseParameters
void parseParameters(const ParametersMap &parameters)
Definition: MarkerDetector.cpp:64
MarkerDetector.h
y
Matrix3f y
corners
void corners(const MatrixType &m)
rtabmap::ParametersMap
std::map< std::string, std::string > ParametersMap
Definition: Parameters.h:43
rtabmap::Parameters::parse
static bool parse(const ParametersMap &parameters, const std::string &key, bool &value)
Definition: Parameters.cpp:500
fabs
Real fabs(const Real &a)
rtabmap::Transform::prettyPrint
std::string prettyPrint() const
Definition: Transform.cpp:326
rtabmap::MarkerDetector::~MarkerDetector
virtual ~MarkerDetector()
Definition: MarkerDetector.cpp:60
scale
set noclip points set clip one set noclip two set bar set border lt lw set xdata set ydata set zdata set x2data set y2data set boxwidth set dummy y set format x g set format y g set format x2 g set format y2 g set format z g set angles radians set nogrid set key title set key left top Right noreverse box linetype linewidth samplen spacing width set nolabel set noarrow set nologscale set logscale x set set pointsize set encoding default set nopolar set noparametric set set set set surface set nocontour set clabel set mapping cartesian set nohidden3d set cntrparam order set cntrparam linear set cntrparam levels auto set cntrparam points set size set set xzeroaxis lt lw set x2zeroaxis lt lw set yzeroaxis lt lw set y2zeroaxis lt lw set tics in set ticslevel set tics scale
ULogger::kWarning
@ kWarning
Definition: ULogger.h:252
UASSERT
#define UASSERT(condition)
d
d
x
x
error
void error(const char *str)
rtabmap::util2d::getDepth
float RTABMAP_CORE_EXPORT getDepth(const cv::Mat &depthImage, float x, float y, bool smoothing, float depthErrorRatio=0.02f, bool estWithNeighborsIfNull=false)
Definition: util2d.cpp:947
UWARN
#define UWARN(...)
f
Point2(* f)(const Point3 &, OptionalJacobian< 2, 3 >)
d2
d2
ULogger::level
static ULogger::Level level()
Definition: ULogger.h:340
ULogger.h
ULogger class and convenient macros.
rtabmap::Transform
Definition: Transform.h:41
util2d.h
iter
iterator iter(handle obj)
c_str
const char * c_str(Args &&...args)
UDEBUG
#define UDEBUG(...)
float
float
rtabmap::MarkerDetector::MarkerDetector
MarkerDetector(const ParametersMap &parameters=ParametersMap())
Definition: MarkerDetector.cpp:34
t
Point2 t(10, 10)
rtabmap
Definition: CameraARCore.cpp:35
UERROR
#define UERROR(...)
trace.model
model
Definition: trace.py:4
i
int i
R
R

rtabmap
Author(s): Mathieu Labbe
autogenerated on Thu Jul 25 2024 02:50:12