markerdetector.h
Go to the documentation of this file.
1 /*****************************
2 Copyright 2011 Rafael Muñoz Salinas. All rights reserved.
3 
4 Redistribution and use in source and binary forms, with or without modification, are
5 permitted provided that the following conditions are met:
6 
7  1. Redistributions of source code must retain the above copyright notice, this list of
8  conditions and the following disclaimer.
9 
10  2. Redistributions in binary form must reproduce the above copyright notice, this list
11  of conditions and the following disclaimer in the documentation and/or other materials
12  provided with the distribution.
13 
14 THIS SOFTWARE IS PROVIDED BY Rafael Muñoz Salinas ''AS IS'' AND ANY EXPRESS OR IMPLIED
15 WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND
16 FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL Rafael Muñoz Salinas OR
17 CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
18 CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
19 SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON
20 ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
21 NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF
22 ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
23 
24 The views and conclusions contained in the software and documentation are those of the
25 authors and should not be interpreted as representing official policies, either expressed
26 or implied, of Rafael Muñoz Salinas.
27 ********************************/
28 #ifndef _ARUCO_MarkerDetector_H
29 #define _ARUCO_MarkerDetector_H
30 #include <opencv2/core/core.hpp>
31 #include <cstdio>
32 #include <iostream>
33 #include "cameraparameters.h"
34 #include "exports.h"
35 #include "marker.h"
36 using namespace std;
37 
38 namespace aruco {
39 
44  // Represent a candidate to be a maker
45  class MarkerCandidate : public Marker {
46  public:
48  MarkerCandidate(const Marker &M) : Marker(M) {}
50  contour = M.contour;
51  idx = M.idx;
52  }
54  (*(Marker *)this) = (*(Marker *)&M);
55  contour = M.contour;
56  idx = M.idx;
57  return *this;
58  }
59 
60  vector< cv::Point > contour; // all the points of its contour
61  int idx; // index position in the global contour list
62  };
63 
64  public:
69 
72  ~MarkerDetector();
73 
85  void detect(const cv::Mat &input, std::vector< Marker > &detectedMarkers, cv::Mat camMatrix = cv::Mat(), cv::Mat distCoeff = cv::Mat(),
86  float markerSizeMeters = -1, bool setYPerperdicular = false) throw(cv::Exception);
97  void detect(const cv::Mat &input, std::vector< Marker > &detectedMarkers, CameraParameters camParams, float markerSizeMeters = -1,
98  bool setYPerperdicular = false) throw(cv::Exception);
99 
103  enum ThresholdMethods { FIXED_THRES, ADPT_THRES, CANNY };
104 
105 
106 
109  void setThresholdMethod(ThresholdMethods m) { _thresMethod = m; }
112  ThresholdMethods getThresholdMethod() const { return _thresMethod; }
119  void setThresholdParams(double param1, double param2) {
120  _thresParam1 = param1;
121  _thresParam2 = param2;
122  }
123 
130  void setThresholdParamRange(size_t r1 = 0, size_t r2 = 0) { _thresParam1_range = r1; }
142  void enableLockedCornersMethod(bool enable);
143 
150  void getThresholdParams(double &param1, double &param2) const {
151  param1 = _thresParam1;
152  param2 = _thresParam2;
153  }
154 
155 
159  const cv::Mat &getThresholdedImage() { return thres; }
162  enum CornerRefinementMethod { NONE, HARRIS, SUBPIX, LINES };
165  void setCornerRefinementMethod(CornerRefinementMethod method) { _cornerMethod = method; }
168  CornerRefinementMethod getCornerRefinementMethod() const { return _cornerMethod; }
175  void setMinMaxSize(float min = 0.03, float max = 0.5) throw(cv::Exception);
176 
182  void getMinMaxSize(float &min, float &max) {
183  min = _minSize;
184  max = _maxSize;
185  }
186 
192  void enableErosion(bool enable) {}
193 
201  void setDesiredSpeed(int val);
204  int getDesiredSpeed() const { return _speed; }
205 
210  void setWarpSize(int val) throw(cv::Exception);
211  ;
214  int getWarpSize() const { return _markerWarpSize; }
215 
232  void setMakerDetectorFunction(int (*markerdetector_func)(const cv::Mat &in, int &nRotations)) { markerIdDetector_ptrfunc = markerdetector_func; }
233 
242  void pyrDown(unsigned int level) {}
243 
248 
252  void thresHold(int method, const cv::Mat &grey, cv::Mat &thresImg, double param1 = -1, double param2 = -1) throw(cv::Exception);
257  void detectRectangles(const cv::Mat &thresImg, vector< std::vector< cv::Point2f > > &candidates);
258 
261  const vector< std::vector< cv::Point2f > > &getCandidates() { return _candidates; }
262 
270  bool warp(cv::Mat &in, cv::Mat &out, cv::Size size, std::vector< cv::Point2f > points) throw(cv::Exception);
271 
272 
273 
277  void refineCandidateLines(MarkerCandidate &candidate, const cv::Mat &camMatrix, const cv::Mat &distCoeff);
278 
279 
294  static void glGetProjectionMatrix(CameraParameters &CamMatrix, cv::Size orgImgSize, cv::Size size, double proj_matrix[16], double gnear, double gfar,
295  bool invert = false) throw(cv::Exception);
296 
297  private:
298  bool warp_cylinder(cv::Mat &in, cv::Mat &out, cv::Size size, MarkerCandidate &mc) throw(cv::Exception);
303  void detectRectangles(vector< cv::Mat > &vimages, vector< MarkerCandidate > &candidates);
304  // Current threshold method
306  // Threshold parameters
307  double _thresParam1, _thresParam2, _thresParam1_range;
308  // Current corner method
310  // minimum and maximum size of a contour lenght
311  float _minSize, _maxSize;
312 
313  // is corner locked
315 
316  // Speed control
317  int _speed;
319  float _borderDistThres; // border around image limits in which corners are not allowed to be detected.
320  // vectr of candidates to be markers. This is a vector with a set of rectangles that have no valid id
321  vector< std::vector< cv::Point2f > > _candidates;
322  // Images
323  cv::Mat grey, thres;
324  // pointer to the function that analizes a rectangular region so as to detect its internal marker
325  int (*markerIdDetector_ptrfunc)(const cv::Mat &in, int &nRotations);
326 
329  bool isInto(cv::Mat &contour, std::vector< cv::Point2f > &b);
332  int perimeter(std::vector< cv::Point2f > &a);
333 
334 
335  // //GL routines
336  //
337  // static void argConvGLcpara2( double cparam[3][4], int width, int height, double gnear, double gfar, double m[16], bool invert )throw(cv::Exception);
338  // static int arParamDecompMat( double source[3][4], double cpara[3][4], double trans[3][4] )throw(cv::Exception);
339  // static double norm( double a, double b, double c );
340  // static double dot( double a1, double a2, double a3,
341  // double b1, double b2, double b3 );
342  //
343 
344  // detection of the
345  void findBestCornerInRegion_harris(const cv::Mat &grey, vector< cv::Point2f > &Corners, int blockSize);
346 
347 
348  // auxiliar functions to perform LINES refinement
349  void interpolate2Dline(const vector< cv::Point2f > &inPoints, cv::Point3f &outLine);
350  cv::Point2f getCrossPoint(const cv::Point3f &line1, const cv::Point3f &line2);
351  void distortPoints(vector< cv::Point2f > in, vector< cv::Point2f > &out, const cv::Mat &camMatrix, const cv::Mat &distCoeff);
352 
353 
359  template < typename T > void removeElements(vector< T > &vinout, const vector< bool > &toRemove) {
360  // remove the invalid ones by setting the valid in the positions left by the invalids
361  size_t indexValid = 0;
362  for (size_t i = 0; i < toRemove.size(); i++) {
363  if (!toRemove[i]) {
364  if (indexValid != i)
365  vinout[indexValid] = vinout[i];
366  indexValid++;
367  }
368  }
369  vinout.resize(indexValid);
370  }
371 
372  // graphical debug
373  void drawApproxCurve(cv::Mat &in, std::vector< cv::Point > &approxCurve, cv::Scalar color);
374  void drawContour(cv::Mat &in, std::vector< cv::Point > &contour, cv::Scalar);
375  void drawAllContours(cv::Mat input, std::vector< std::vector< cv::Point > > &contours);
376  void draw(cv::Mat out, const std::vector< Marker > &markers);
377  // method to refine corner detection in case the internal border after threshold is found
378  // This was tested in the context of chessboard methods
379  void findCornerMaxima(vector< cv::Point2f > &Corners, const cv::Mat &grey, int wsize);
380 
381 
382 
383  template < typename T > void joinVectors(vector< vector< T > > &vv, vector< T > &v, bool clearv = false) {
384  if (clearv)
385  v.clear();
386  for (size_t i = 0; i < vv.size(); i++)
387  for (size_t j = 0; j < vv[i].size(); j++)
388  v.push_back(vv[i][j]);
389  }
390 };
391 };
392 #endif
void setThresholdParams(double param1, double param2)
void getMinMaxSize(float &min, float &max)
#define ARUCO_EXPORTS
Definition: exports.h:42
void pyrDown(unsigned int level)
CornerRefinementMethod _cornerMethod
const vector< std::vector< cv::Point2f > > & getCandidates()
void removeElements(vector< T > &vinout, const vector< bool > &toRemove)
CornerRefinementMethod getCornerRefinementMethod() const
MarkerCandidate & operator=(const MarkerCandidate &M)
void setThresholdParamRange(size_t r1=0, size_t r2=0)
MarkerCandidate(const MarkerCandidate &M)
ThresholdMethods _thresMethod
This class represents a marker. It is a vector of the fours corners ot the marker.
Definition: marker.h:41
Main class for marker detection.
const cv::Mat & getThresholdedImage()
Parameters of the camera.
void getThresholdParams(double &param1, double &param2) const
ThresholdMethods getThresholdMethod() const
void enableErosion(bool enable)
int min(int a, int b)
void joinVectors(vector< vector< T > > &vv, vector< T > &v, bool clearv=false)
void setMakerDetectorFunction(int(*markerdetector_func)(const cv::Mat &in, int &nRotations))
void setThresholdMethod(ThresholdMethods m)
void setCornerRefinementMethod(CornerRefinementMethod method)
int getDesiredSpeed() const
vector< std::vector< cv::Point2f > > _candidates


asr_aruco_marker_recognition
Author(s): Allgeyer Tobias, Meißner Pascal, Qattan Mohamad
autogenerated on Mon Feb 28 2022 21:41:30