markerdetector.h

Go to the documentation of this file.

/*****************************
Copyright 2011 Rafael Muñoz Salinas. All rights reserved.

Redistribution and use in source and binary forms, with or without modification, are
permitted provided that the following conditions are met:

   1. Redistributions of source code must retain the above copyright notice, this list of
      conditions and the following disclaimer.

   2. Redistributions in binary form must reproduce the above copyright notice, this list
      of conditions and the following disclaimer in the documentation and/or other materials
      provided with the distribution.

THIS SOFTWARE IS PROVIDED BY Rafael Muñoz Salinas ''AS IS'' AND ANY EXPRESS OR IMPLIED
WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND
FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL Rafael Muñoz Salinas OR
CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON
ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF
ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

The views and conclusions contained in the software and documentation are those of the
authors and should not be interpreted as representing official policies, either expressed
or implied, of Rafael Muñoz Salinas.
********************************/
#ifndef _ARUCO_MarkerDetector_H
#define _ARUCO_MarkerDetector_H
#include <opencv2/core/core.hpp>
#include <cstdio>
#include <iostream>
#include "cameraparameters.h"
#include "exports.h"
#include "marker.h"
using namespace std;

namespace aruco {

class ARUCO_EXPORTS MarkerDetector {
    // Represent a candidate to be a maker
    class MarkerCandidate : public Marker {
      public:
        MarkerCandidate() {}
        MarkerCandidate(const Marker &M) : Marker(M) {}
        MarkerCandidate(const MarkerCandidate &M) : Marker(M) {
            contour = M.contour;
            idx = M.idx;
        }
        MarkerCandidate &operator=(const MarkerCandidate &M) {
            (*(Marker *)this) = (*(Marker *)&M);
            contour = M.contour;
            idx = M.idx;
            return *this;
        }

        vector< cv::Point > contour; // all the points of its contour
        int idx; // index position in the global contour list
    };

  public:
    MarkerDetector();

    ~MarkerDetector();

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

    enum ThresholdMethods { FIXED_THRES, ADPT_THRES, CANNY };



    void setThresholdMethod(ThresholdMethods m) { _thresMethod = m; }
    ThresholdMethods getThresholdMethod() const { return _thresMethod; }
    void setThresholdParams(double param1, double param2) {
        _thresParam1 = param1;
        _thresParam2 = param2;
    }

    void setThresholdParamRange(size_t r1 = 0, size_t r2 = 0) { _thresParam1_range = r1; }
    void enableLockedCornersMethod(bool enable);

    void getThresholdParams(double &param1, double &param2) const {
        param1 = _thresParam1;
        param2 = _thresParam2;
    }


    const cv::Mat &getThresholdedImage() { return thres; }
    enum CornerRefinementMethod { NONE, HARRIS, SUBPIX, LINES };
    void setCornerRefinementMethod(CornerRefinementMethod method) { _cornerMethod = method; }
    CornerRefinementMethod getCornerRefinementMethod() const { return _cornerMethod; }
    void setMinMaxSize(float min = 0.03, float max = 0.5) throw(cv::Exception);

    void getMinMaxSize(float &min, float &max) {
        min = _minSize;
        max = _maxSize;
    }

    void enableErosion(bool enable) {}

    void setDesiredSpeed(int val);
    int getDesiredSpeed() const { return _speed; }

    void setWarpSize(int val) throw(cv::Exception);
    ;
    int getWarpSize() const { return _markerWarpSize; }

    void setMakerDetectorFunction(int (*markerdetector_func)(const cv::Mat &in, int &nRotations)) { markerIdDetector_ptrfunc = markerdetector_func; }

    void pyrDown(unsigned int level) {}


    void thresHold(int method, const cv::Mat &grey, cv::Mat &thresImg, double param1 = -1, double param2 = -1) throw(cv::Exception);
    void detectRectangles(const cv::Mat &thresImg, vector< std::vector< cv::Point2f > > &candidates);

    const vector< std::vector< cv::Point2f > > &getCandidates() { return _candidates; }

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



    void refineCandidateLines(MarkerCandidate &candidate, const cv::Mat &camMatrix, const cv::Mat &distCoeff);


    static void glGetProjectionMatrix(CameraParameters &CamMatrix, cv::Size orgImgSize, cv::Size size, double proj_matrix[16], double gnear, double gfar,
                                      bool invert = false) throw(cv::Exception);

  private:
    bool warp_cylinder(cv::Mat &in, cv::Mat &out, cv::Size size, MarkerCandidate &mc) throw(cv::Exception);
    void detectRectangles(vector< cv::Mat > &vimages, vector< MarkerCandidate > &candidates);
    // Current threshold method
    ThresholdMethods _thresMethod;
    // Threshold parameters
    double _thresParam1, _thresParam2, _thresParam1_range;
    // Current corner method
    CornerRefinementMethod _cornerMethod;
    // minimum and maximum size of a contour lenght
    float _minSize, _maxSize;

    // is corner locked
    bool _useLockedCorners;

    // Speed control
    int _speed;
    int _markerWarpSize;
    float _borderDistThres; // border around image limits in which corners are not allowed to be detected.
    // vectr of candidates to be markers. This is a vector with a set of rectangles that have no valid id
    vector< std::vector< cv::Point2f > > _candidates;
    // Images
    cv::Mat grey, thres;
    // pointer to the function that analizes a rectangular region so as to detect its internal marker
    int (*markerIdDetector_ptrfunc)(const cv::Mat &in, int &nRotations);

    bool isInto(cv::Mat &contour, std::vector< cv::Point2f > &b);
    int perimeter(std::vector< cv::Point2f > &a);


    //     //GL routines
    //
    //     static void argConvGLcpara2( double cparam[3][4], int width, int height, double gnear, double gfar, double m[16], bool invert )throw(cv::Exception);
    //     static int  arParamDecompMat( double source[3][4], double cpara[3][4], double trans[3][4] )throw(cv::Exception);
    //     static double norm( double a, double b, double c );
    //     static double dot(  double a1, double a2, double a3,
    //                         double b1, double b2, double b3 );
    //

    // detection of the
    void findBestCornerInRegion_harris(const cv::Mat &grey, vector< cv::Point2f > &Corners, int blockSize);


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


    template < typename T > void removeElements(vector< T > &vinout, const 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);
    }

    // graphical debug
    void drawApproxCurve(cv::Mat &in, std::vector< cv::Point > &approxCurve, cv::Scalar color);
    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);
    // method to refine corner detection in case the internal border after threshold is found
    // This was tested in the context of chessboard methods
    void findCornerMaxima(vector< cv::Point2f > &Corners, const cv::Mat &grey, int wsize);



    template < typename T > void joinVectors(vector< vector< T > > &vv, 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]);
    }
};
};
#endif