Program Listing for File markerdetector_impl.h

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

#ifndef _ARUCO_MarkerDetector_Impl_H
#define _ARUCO_MarkerDetector_Impl_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 "markerdetector.h"
#include <opencv2/imgproc/imgproc.hpp>

namespace aruco
{

class CameraParameters;
class MarkerLabeler;
class MarkerDetector_Impl
{
  friend class MarkerDetector;

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

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

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


  ~MarkerDetector_Impl();
  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);


  MarkerDetector::Params getParameters() const
  {
    return _params;
  }
  MarkerDetector::Params& getParameters()
  {
    return _params;
  }
  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()
  {
    return markerIdDetector;
  }
  typedef Marker MarkerCandidate;


  std::vector<MarkerCandidate> getCandidates() const
  {
    return _candidates_wcontour;
  }

  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 MarkerDetector::Params& params);

  std::vector<cv::Mat> getImagePyramid()
  {
    return imagePyramid;
  }



private:
  // obfuscate start

  // operating params
  MarkerDetector::Params _params;

  // Images
  cv::Mat grey, thres;
  // pointer to the function that analizes a rectangular region so as to detect its internal marker
  cv::Ptr<MarkerLabeler> markerIdDetector;
  int perimeter(const std::vector<cv::Point2f>& a);

  // auxiliar functions to perform LINES refinement
  void interpolate2Dline(const std::vector<cv::Point2f>& inPoints, cv::Point3f& outLine);
  cv::Point2f getCrossPoint(const cv::Point3f& line1, const cv::Point3f& line2);
  void distortPoints(std::vector<cv::Point2f> in, std::vector<cv::Point2f>& out,
                     const cv::Mat& camMatrix, const cv::Mat& distCoeff);

  // returns the number of pixels that the smallest and largest allowed markers have
  int getMinMarkerSizePix(cv::Size orginput_imageSize) const;

  // returns the markerWarpSize
  int getMarkerWarpSize();
  template <typename T>
  void removeElements(std::vector<T>& vinout, const std::vector<bool>& toRemove)
  {
    // remove the invalid ones by setting the valid in the positions left by the invalids
    size_t indexValid = 0;
    for (size_t i = 0; i < toRemove.size(); i++)
    {
      if (!toRemove[i])
      {
        if (indexValid != i)
          vinout[indexValid] = vinout[i];
        indexValid++;
      }
    }
    vinout.resize(indexValid);
  }


  template <typename T>
  void joinVectors(std::vector<std::vector<T>>& vv, std::vector<T>& v, bool clearv = false)
  {
    if (clearv)
      v.clear();
    for (size_t i = 0; i < vv.size(); i++)
      for (size_t j = 0; j < vv[i].size(); j++)
        v.push_back(vv[i][j]);
  }

  std::vector<cv::Mat> imagePyramid;
  void enlargeMarkerCandidate(MarkerCandidate& cand, int fact = 1);

  void cornerUpsample(std::vector<std::vector<cv::Point2f>>& MarkerCanditates,
                      cv::Size lowResImageSize);
  void cornerUpsample(std::vector<Marker>& MarkerCanditates, cv::Size lowResImageSize);



  void buildPyramid(std::vector<cv::Mat>& imagePyramid, const cv::Mat& grey, int minSize);



  std::vector<aruco::MarkerCandidate> thresholdAndDetectRectangles(const cv::Mat& input,
                                                                   int thres_param1,
                                                                   int thres_param2, bool erode,
                                                                   cv::Mat& auxThresImage);
  std::vector<aruco::MarkerCandidate> thresholdAndDetectRectangles(const cv::Mat& image);
  std::vector<aruco::MarkerCandidate> prefilterCandidates(std::vector<MarkerCandidate>& candidates,
                                                          cv::Size orgImageSize);


  std::vector<cv::Mat> _thres_Images;
  std::vector<std::vector<MarkerCandidate>> _vcandidates;
  // std::vector<std::vector<cv::Point2f > > _candidates;
  std::vector<MarkerCandidate> _candidates_wcontour;
  std::vector<Marker> _prevMarkers;       // employed for tracking
  std::map<int, int> marker_ndetections;  // used to keeps track only of markers with a
                                          // minimum number of detections

  // graphical debug
  void drawApproxCurve(cv::Mat& in, std::vector<cv::Point>& approxCurve, cv::Scalar color,
                       int thickness = 1);
  void drawContour(cv::Mat& in, std::vector<cv::Point>& contour, cv::Scalar);
  void drawAllContours(cv::Mat input, std::vector<std::vector<cv::Point>>& contours);
  void draw(cv::Mat out, const std::vector<Marker>& markers);



  enum ThreadTasks
  {
    THRESHOLD_TASK,
    ENCLOSE_TASK,
    EXIT_TASK
  };
  struct ThresAndDetectRectTASK
  {
    int inIdx, outIdx;
    int param1, param2;
    ThreadTasks task;
  };
  void thresholdAndDetectRectangles_thread();

  // thread safe queue to implement producer-consumer
  template <typename T>
  class Queue
  {
  public:
    T pop()
    {
      std::unique_lock<std::mutex> mlock(mutex_);
      while (queue_.empty())
      {
        cond_.wait(mlock);
      }
      auto item = queue_.front();
      queue_.pop();
      return item;
    }

    void push(const T& item)
    {
      std::unique_lock<std::mutex> mlock(mutex_);
      queue_.push(item);
      mlock.unlock();
      cond_.notify_one();
    }

    size_t size()
    {
      std::unique_lock<std::mutex> mlock(mutex_);
      size_t s = queue_.size();
      return s;
    }

  private:
    std::queue<T> queue_;
    std::mutex mutex_;
    std::condition_variable cond_;
  };
  Queue<ThresAndDetectRectTASK> _tasks;
  void refineCornerWithContourLines(aruco::Marker& marker, cv::Mat cameraMatrix = cv::Mat(),
                                    cv::Mat distCoef = cv::Mat());

  inline float pointSqdist(cv::Point& p, cv::Point2f& p2)
  {
    float dx = p.x - p2.x;
    float dy = p.y - p2.y;
    return dx * dx + dy * dy;
  }

  float _tooNearDistance = -1;  // pixel distance between nearr rectangle. Computed
                                // automatically based on the params



  struct marker_analyzer
  {
    void setParams(const std::vector<cv::Point2f>& m)
    {
      corners = m;
      bax = m[1].x - m[0].x;
      bay = m[1].y - m[0].y;
      dax = m[2].x - m[0].x;
      day = m[2].y - m[0].y;
      a = m[0];
      b = m[1];
      d = m[2];
      area = _getArea();

      center = cv::Point2f(0, 0);
      for (auto& p : corners)
        center += p;
      center *= 1. / 4.;
    }

    bool isInto(const cv::Point2f& p) const
    {
      if (signD(corners[0], corners[1], p) < 0)
        return false;
      if (signD(corners[1], corners[2], p) < 0)
        return false;
      if (signD(corners[2], corners[3], p) < 0)
        return false;
      if (signD(corners[3], corners[0], p) < 0)
        return false;
      return true;
    }
    cv::Point2f getCenter() const
    {
      return center;
    }
    float getArea() const
    {
      return area;
    }
    float _getArea() const
    {
      // use the cross products
      cv::Point2f v01 = corners[1] - corners[0];
      cv::Point2f v03 = corners[3] - corners[0];
      float area1 = fabs(v01.x * v03.y - v01.y * v03.x);
      cv::Point2f v21 = corners[1] - corners[2];
      cv::Point2f v23 = corners[3] - corners[2];
      float area2 = fabs(v21.x * v23.y - v21.y * v23.x);
      return (area2 + area1) / 2.f;
    }
    float bax, bay, dax, day;
    cv::Point2f a, b, d;
    float area;
    cv::Point2f center;

    std::vector<cv::Point2f> corners;

    inline float signD(cv::Point2f p0, cv::Point2f p1, cv::Point2f p) const
    {
      return ((p0.y - p1.y) * p.x + (p1.x - p0.x) * p.y + (p0.x * p1.y - p1.x * p0.y)) /
             sqrt((p1.x - p0.x) * (p1.x - p0.x) + (p1.y - p0.y) * (p1.y - p0.y));
    }
  };
  void addToImageHist(cv::Mat& im, std::vector<float>& hist);
  int Otsu(std::vector<float>& hist);

  void filter_ambiguous_query(std::vector<cv::DMatch>& matches);
  // obfuscate end
};
};  // namespace aruco
#endif