00001 #ifndef CV_ARUCO_H
00002 #define CV_ARUCO_H
00003 #include <opencv/cv.h>
00004 #include <opencv/highgui.h>
00005 #include <cstdio>
00006 #include <iostream>
00007 #include "cameraparameters.h"
00008 #include "marker.h"
00009 using namespace std;
00010
00011 namespace aruco
00012 {
00013
00017 class MarkerDetector
00018 {
00019 public:
00020
00023 MarkerDetector();
00024
00027 ~MarkerDetector();
00028
00039 void detect(cv::Mat &input,std::vector<Marker> &detectedMarkers,cv::Mat camMatrix=cv::Mat(),cv::Mat distCoeff=cv::Mat(),float markerSizeMeters=-1) throw (cv::Exception);
00049 void detect(cv::Mat &input,std::vector<Marker> &detectedMarkers, CameraParameters camParams=CameraParameters(),float markerSizeMeters=-1) throw (cv::Exception);
00050
00054 enum ThresholdMethods {FIXED_THRES,ADPT_THRES,CANNY};
00055
00058 void setThresholdMethod(ThresholdMethods m) {
00059 _thresMethod=m;
00060 }
00063 ThresholdMethods getThresholdMethod()const {
00064 return _thresMethod;
00065 }
00072 void setThresholdParams(double param1,double param2) {
00073 _thresParam1=param1;
00074 _thresParam2=param2;
00075 }
00082 void getThresholdParams(double ¶m1,double ¶m2)const {
00083 param1=_thresParam1;
00084 param2=_thresParam2;
00085 }
00086
00087
00091 cv::Mat & getThresholdedImage() {
00092 return thres;
00093 }
00094
00095
00107 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);
00108 private:
00109
00110 cv::Mat grey,thres,thres2;
00111 std::vector<std::vector<cv::Point> > contours2;
00112 std::vector<cv::Vec4i> hierarchy2;
00113
00114 double _thresParam1,_thresParam2;
00115
00116 ThresholdMethods _thresMethod;
00123 void warp(cv::Mat &in,cv::Mat &out,cv::Size size, std::vector<cv::Point2f> points)throw (cv::Exception);
00124
00125 int hammDistMarker(cv::Mat bits);
00126 int mat2id(cv::Mat &bits);
00129 bool correctHammMarker(cv::Mat &bits);
00130
00131 cv::Mat rotate(cv::Mat in);
00132
00136 int getMarkerId(cv::Mat &in,int &nRotations);
00137
00138 void drawApproxCurve(cv::Mat &in,std::vector<cv::Point> &approxCurve ,cv::Scalar color);
00139 void drawContour(cv::Mat &in,std::vector<cv::Point> &contour,cv::Scalar );
00140 void thresHold(int method,cv::Mat &grey,cv::Mat &out);
00141
00142 void drawAllContours(cv::Mat input);
00143
00144 void draw(cv::Mat out,const std::vector<Marker> &markers );
00147 bool isInto(cv::Mat &contour,std::vector<cv::Point2f> &b);
00150
00153 int perimeter(std::vector<cv::Point2f> &a);
00156 template<typename T>
00157 void printMat(cv::Mat M,std::string info="")
00158 {
00159
00160 std::cout<<info<<std::endl;
00161 for (int y=0;y<M.rows;y++)
00162 {
00163 for (int x=0;x<M.cols;x++)
00164 {
00165 if (sizeof(T)==1)
00166 std::cout<<(int) M.at<T>(y,x)<<" ";
00167 else std::cout<< M.at<T>(y,x)<<" ";
00168 }
00169 std::cout<<std::endl;
00170 }
00171 }
00174 template<typename T>
00175 void printMat(CvMat *M,std::string info="")
00176 {
00177 std::cout<<info<<std::endl;
00178 cv::Mat MM(M);
00179 for (int y=0;y<MM.rows;y++)
00180 {
00181 for (int x=0;x<MM.cols;x++)
00182 {
00183 if (sizeof(T)==1)
00184 std::cout<<(int) MM.at<T>(y,x)<<" ";
00185 else std::cout<< MM.at<T>(y,x)<<" ";
00186 }
00187 std::cout<<endl;
00188 }
00189 }
00190
00191
00192
00193 static void argConvGLcpara2( double cparam[3][4], int width, int height, double gnear, double gfar, double m[16], bool invert )throw(cv::Exception);
00194 static int arParamDecompMat( double source[3][4], double cpara[3][4], double trans[3][4] )throw(cv::Exception);
00195 static double norm( double a, double b, double c );
00196 static double dot( double a1, double a2, double a3,
00197 double b1, double b2, double b3 );
00198
00199
00200
00201 };
00202
00203
00204
00205
00206 };
00207 #endif