Program Listing for File markerdetector.h

Return to documentation for file (/tmp/ws/src/aruco_ros/aruco/include/aruco/markerdetector.h)

#ifndef _ARUCO_MarkerDetector_H
#define _ARUCO_MarkerDetector_H

#include "aruco_export.h"
#include <opencv2/core/core.hpp>
#include <cstdio>
#include <iostream>
#include <queue>
#include <mutex>
#include <condition_variable>
#include <vector>
#include <map>
#include "marker.h"

#include <opencv2/imgproc/imgproc.hpp>

namespace aruco
{

enum DetectionMode : int
{
  DM_NORMAL = 0,
  DM_FAST = 1,
  DM_VIDEO_FAST = 2
};
enum CornerRefinementMethod : int
{
  CORNER_SUBPIX = 0,
  CORNER_LINES = 1,
  CORNER_NONE = 2
};


class CameraParameters;
class MarkerLabeler;
class MarkerDetector_Impl;
typedef Marker MarkerCandidate;

class ARUCO_EXPORT MarkerDetector
{
  friend class MarkerDetector_Impl;

public:
  enum ThresMethod : int
  {
    THRES_ADAPTIVE = 0,
    THRES_AUTO_FIXED = 1
  };

  struct ARUCO_EXPORT Params
  {
    void setDetectionMode(DetectionMode dm, float minMarkerSize);

    void detectEnclosedMarkers(bool do_)
    {
      enclosedMarker = do_;
    }

    void setCornerRefinementMethod(CornerRefinementMethod method);


    //-----------------------------------------------------------------------------
    // Below this point you probably should not use the functions
    void setThresholdMethod(ThresMethod method, int thresHold = -1, int wsize = -1,
                            int wsize_range = 0);



    void setAutoSizeSpeedUp(bool v, float Ts = 0.25)
    {
      autoSize = v;
      ts = Ts;
    }
    bool getAutoSizeSpeedUp() const
    {
      return autoSize;
    }



    void save(cv::FileStorage &fs) const;
    void load(cv::FileStorage &fs);

    void toStream(std::ostream &str) const;
    void fromStream(std::istream &str);

    static std::string toString(DetectionMode dm);
    static DetectionMode getDetectionModeFromString(const std::string &str);
    static std::string toString(CornerRefinementMethod dm);
    static CornerRefinementMethod getCornerRefinementMethodFromString(const std::string &str);
    static std::string toString(ThresMethod dm);
    static ThresMethod getCornerThresMethodFromString(const std::string &str);

    // Detection mode

    DetectionMode detectMode = DM_NORMAL;

    // maximum number of parallel threads
    int maxThreads = 1;  //-1 means all

    // border around image limits in which corners are not allowed to be detected. (0,1)
    float borderDistThres = 0.015f;
    int lowResMarkerSize = 20;  // minimum size of a marker in the low resolution image

    // minimum  size of a contour lenght. We use the following formula
    // minLenght=  min ( _minSize_pix , _minSize* Is)*4
    // being Is=max(imageWidth,imageHeight)
    // the value  _minSize are normalized, thus, depends on camera image size
    // However, _minSize_pix is expressed in pixels (you can use the one you prefer)
    float minSize = -1;  // tau_i in paper
    int minSize_pix = -1;
    bool enclosedMarker = false;  // special treatment for enclosed markers
    float error_correction_rate = 0;
    std::string dictionary = "ALL_DICTS";
    // threshold methods
    ThresMethod thresMethod = THRES_ADAPTIVE;
    int NAttemptsAutoThresFix =
        3;  // number of times that tries a random threshold in case of THRES_AUTO_FIXED
    int trackingMinDetections = 0;  // no tracking


    // Threshold parameters
    int AdaptiveThresWindowSize = -1, ThresHold = 7, AdaptiveThresWindowSize_range = 0;
    // size of the image passedta to the MarkerLabeler
    int markerWarpPixSize = 5;  // tau_c in paper

    CornerRefinementMethod cornerRefinementM = CORNER_SUBPIX;
    // enable/disables the method for automatic size estimation for speed up
    bool autoSize = false;
    float ts =
        0.25f;  //$\tau_s$ is a factor in the range $(0,1]$ that accounts for the camera
                //motion speed. For instance, when $\tau_s=0.1$, it means that in the next
                //frame, $\tau_i$ is such that markers $10\%$ smaller than the smallest
                //marker in the current image  will be seek. To avoid loosing track of the
                //markers. If no markers are detected in a frame, $\tau_i$ is set to zero
                //for the next frame so that markers of any size can be detected.
    float pyrfactor = 2;
    int closingSize =
        0;  // enables/disables morph closing operation. The actual param used is closingSize*2+1

  private:
    static void _toStream(const std::string &strg, std::ostream &str);
    static void _fromStream(std::string &strg, std::istream &str);
    template <typename Type>
    static bool attemtpRead(const std::string &name, Type &var, cv::FileStorage &fs)
    {
      if (fs[name].type() != cv::FileNode::NONE)
      {
        fs[name] >> var;
        return true;
      }
      return false;
    }
  };

  MarkerDetector();
  MarkerDetector(int dict_type, float error_correction_rate = 0);
  MarkerDetector(std::string dict_type, float error_correction_rate = 0);

  void saveParamsToFile(const std::string &path) const;

  void loadParamsFromFile(const std::string &path);


  ~MarkerDetector();
  void setDetectionMode(DetectionMode dm, float minMarkerSize = 0);
  DetectionMode getDetectionMode();
  std::vector<aruco::Marker> detect(const cv::Mat &input);
  std::vector<aruco::Marker> detect(const cv::Mat &input, const CameraParameters &camParams,
                                    float markerSizeMeters, bool setYPerperdicular = false,
                                    bool correctFisheye = false);

  void detect(const cv::Mat &input, std::vector<Marker> &detectedMarkers,
              CameraParameters camParams, float markerSizeMeters = -1,
              bool setYPerperdicular = false, bool correctFisheye = false);

  void detect(const cv::Mat &input, std::vector<Marker> &detectedMarkers,
              cv::Mat camMatrix = cv::Mat(), cv::Mat distCoeff = cv::Mat(),
              cv::Mat extrinsics = cv::Mat(), float markerSizeMeters = -1,
              bool setYPerperdicular = false, bool correctFisheye = false);

  Params getParameters() const;
  Params &getParameters();
  void setDictionary(std::string dict_type, float error_correction_rate = 0);

  void setDictionary(int dict_type, float error_correction_rate = 0);

  cv::Mat getThresholdedImage(uint32_t idx = 0);
  //   size_t getNhresholdedImages()const{return _thres_Images.size();}




  void setMarkerLabeler(cv::Ptr<MarkerLabeler> detector);
  cv::Ptr<MarkerLabeler> getMarkerLabeler();


  std::vector<MarkerCandidate> getCandidates() const;

  std::vector<cv::Mat> getImagePyramid();
  /*
   * @param corners vectors of vectors
   */
  void cornerUpsample(std::vector<std::vector<cv::Point2f> > &corners, cv::Size lowResImageSize);
  void cornerUpsample(std::vector<Marker> &corners, cv::Size lowResImageSize);

  bool warp(cv::Mat &in, cv::Mat &out, cv::Size size, std::vector<cv::Point2f> points);


  // serialization in binary mode
  void toStream(std::ostream &str) const;
  void fromStream(std::istream &str);
  // configure the detector from a set of parameters
  void setParameters(const Params &params);



private:
  MarkerDetector_Impl *_impl;
};
};  // namespace aruco
#endif