markerdetector.cpp
Go to the documentation of this file.
00001 /*****************************
00002 Copyright 2011 Rafael Muñoz Salinas. All rights reserved.
00003 
00004 Redistribution and use in source and binary forms, with or without modification, are
00005 permitted provided that the following conditions are met:
00006 
00007    1. Redistributions of source code must retain the above copyright notice, this list of
00008       conditions and the following disclaimer.
00009 
00010    2. Redistributions in binary form must reproduce the above copyright notice, this list
00011       of conditions and the following disclaimer in the documentation and/or other materials
00012       provided with the distribution.
00013 
00014 THIS SOFTWARE IS PROVIDED BY Rafael Muñoz Salinas ''AS IS'' AND ANY EXPRESS OR IMPLIED
00015 WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND
00016 FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL Rafael Muñoz Salinas OR
00017 CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00018 CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
00019 SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON
00020 ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
00021 NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF
00022 ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00023 
00024 The views and conclusions contained in the software and documentation are those of the
00025 authors and should not be interpreted as representing official policies, either expressed
00026 or implied, of Rafael Muñoz Salinas.
00027 ********************************/
00028 #include <aruco/markerdetector.h>
00029 #include <opencv/cv.h>
00030 #include <opencv/highgui.h>
00031 #include <iostream>
00032 #include <fstream>
00033 #include <aruco/arucofidmarkers.h>
00034 #include <valarray>
00035 using namespace std;
00036 using namespace cv;
00037 
00038 namespace aruco
00039 {
00040   /************************************
00041  *
00042  *
00043  *
00044  *
00045  ************************************/
00046   MarkerDetector::MarkerDetector()
00047   {
00048     _doErosion=false;
00049     _enableCylinderWarp=false;
00050     _thresMethod=ADPT_THRES;
00051     _thresParam1=_thresParam2=7;
00052     _cornerMethod=LINES;
00053     _markerWarpSize=56;
00054     _speed=0;
00055     markerIdDetector_ptrfunc=aruco::FiducidalMarkers::detect;
00056     pyrdown_level=0; // no image reduction
00057     _minSize=0.04;
00058     _maxSize=0.5;
00059   }
00060   /************************************
00061  *
00062  *
00063  *
00064  *
00065  ************************************/
00066 
00067   MarkerDetector::~MarkerDetector()
00068   {
00069 
00070   }
00071 
00072   /************************************
00073  *
00074  *
00075  *
00076  *
00077  ************************************/
00078   void MarkerDetector::setDesiredSpeed ( int val )
00079   {
00080     if ( val<0 ) val=0;
00081     else if ( val>3 ) val=2;
00082 
00083     _speed=val;
00084     switch ( _speed )
00085     {
00086 
00087     case 0:
00088       _markerWarpSize=56;
00089       _cornerMethod=SUBPIX;
00090       _doErosion=true;
00091       break;
00092 
00093     case 1:
00094     case 2:
00095       _markerWarpSize=28;
00096       _cornerMethod=NONE;
00097       break;
00098     };
00099   }
00100 
00101   /************************************
00102  *
00103  *
00104  *
00105  *
00106  ************************************/
00107   void MarkerDetector::detect ( const  cv::Mat &input,std::vector<Marker> &detectedMarkers, CameraParameters camParams ,float markerSizeMeters ,bool setYPerpendicular) throw ( cv::Exception )
00108   {
00109     detect ( input, detectedMarkers,camParams.CameraMatrix ,camParams.Distorsion,  markerSizeMeters ,setYPerpendicular);
00110   }
00111 
00112 
00113   /************************************
00114  *
00115  * Main detection function. Performs all steps
00116  *
00117  *
00118  ************************************/
00119   void MarkerDetector::detect ( const  cv::Mat &input,vector<Marker> &detectedMarkers,Mat camMatrix ,Mat distCoeff ,float markerSizeMeters ,bool setYPerpendicular) throw ( cv::Exception )
00120   {
00121 
00122 
00123     //it must be a 3 channel image
00124     if ( input.type() ==CV_8UC3 )   cv::cvtColor ( input,grey,CV_BGR2GRAY );
00125     else     grey=input;
00126 
00127 
00128     //     cv::cvtColor(grey,_ssImC ,CV_GRAY2BGR); //DELETE
00129 
00130     //clear input data
00131     detectedMarkers.clear();
00132 
00133 
00134     cv::Mat imgToBeThresHolded=grey;
00135     double ThresParam1=_thresParam1,ThresParam2=_thresParam2;
00136     //Must the image be downsampled before continue processing?
00137     if ( pyrdown_level!=0 )
00138     {
00139       reduced=grey;
00140       for ( int i=0;i<pyrdown_level;i++ )
00141       {
00142         cv::Mat tmp;
00143         cv::pyrDown ( reduced,tmp );
00144         reduced=tmp;
00145       }
00146       int red_den=pow ( 2.0f,pyrdown_level );
00147       imgToBeThresHolded=reduced;
00148       ThresParam1/=float ( red_den );
00149       ThresParam2/=float ( red_den );
00150     }
00151 
00153     thresHold ( _thresMethod,imgToBeThresHolded,thres,ThresParam1,ThresParam2 );
00154     //an erosion might be required to detect chessboard like boards
00155 
00156     if ( _doErosion )
00157     {
00158       erode ( thres,thres2,cv::Mat() );
00159       thres2=thres;
00160       //         cv::bitwise_xor ( thres,thres2,thres );
00161     }
00162     //find all rectangles in the thresholdes image
00163     vector<MarkerCandidate > MarkerCanditates;
00164     detectRectangles ( thres,MarkerCanditates );
00165     //if the image has been downsampled, then calcualte the location of the corners in the original image
00166     if ( pyrdown_level!=0 )
00167     {
00168       float red_den=pow ( 2.0f,pyrdown_level );
00169       float offInc= ( ( pyrdown_level/2. )-0.5 );
00170       for ( unsigned int i=0;i<MarkerCanditates.size();++i ) {
00171         for ( int c=0;c<4;c++ )
00172         {
00173           MarkerCanditates[i][c].x=MarkerCanditates[i][c].x*red_den+offInc;
00174           MarkerCanditates[i][c].y=MarkerCanditates[i][c].y*red_den+offInc;
00175         }
00176         //do the same with the the contour points
00177         for ( size_t c=0;c<MarkerCanditates[i].contour.size();++c )
00178         {
00179           MarkerCanditates[i].contour[c].x=MarkerCanditates[i].contour[c].x*red_den+offInc;
00180           MarkerCanditates[i].contour[c].y=MarkerCanditates[i].contour[c].y*red_den+offInc;
00181         }
00182       }
00183     }
00184 
00186     _candidates.clear();
00187     for ( unsigned int i=0;i<MarkerCanditates.size();++i )
00188     {
00189       //Find proyective homography
00190       Mat canonicalMarker;
00191       bool resW=false;
00192       if (_enableCylinderWarp)
00193         resW=warp_cylinder( grey,canonicalMarker,Size ( _markerWarpSize,_markerWarpSize ),MarkerCanditates[i] );
00194       else  resW=warp ( grey,canonicalMarker,Size ( _markerWarpSize,_markerWarpSize ),MarkerCanditates[i] );
00195       if (resW) {
00196         int nRotations;
00197         int id= ( *markerIdDetector_ptrfunc ) ( canonicalMarker,nRotations );
00198         if ( id!=-1 )
00199         {
00200           if(_cornerMethod==LINES) refineCandidateLines( MarkerCanditates[i] ); // make LINES refinement before lose contour points
00201           detectedMarkers.push_back ( MarkerCanditates[i] );
00202           detectedMarkers.back().id=id;
00203           //sort the points so that they are always in the same order no matter the camera orientation
00204           std::rotate ( detectedMarkers.back().begin(),detectedMarkers.back().begin() +4-nRotations,detectedMarkers.back().end() );
00205         }
00206         else _candidates.push_back ( MarkerCanditates[i] );
00207       }
00208 
00209     }
00210 
00211 
00213     if ( detectedMarkers.size() >0 && _cornerMethod!=NONE && _cornerMethod!=LINES )
00214     {
00215       vector<Point2f> Corners;
00216       for ( unsigned int i=0;i<detectedMarkers.size();++i )
00217         for ( int c=0;c<4;c++ )
00218           Corners.push_back ( detectedMarkers[i][c] );
00219 
00220       if ( _cornerMethod==HARRIS )
00221         findBestCornerInRegion_harris ( grey, Corners,7 );
00222       else if ( _cornerMethod==SUBPIX )
00223         cornerSubPix ( grey, Corners,cvSize ( 5,5 ), cvSize ( -1,-1 )   ,cvTermCriteria ( CV_TERMCRIT_ITER|CV_TERMCRIT_EPS,3,0.05 ) );
00224 
00225       //copy back
00226       for ( unsigned int i=0;i<detectedMarkers.size();++i )
00227         for ( int c=0;c<4;c++ )     detectedMarkers[i][c]=Corners[i*4+c];
00228     }
00229     //sort by id
00230     std::sort ( detectedMarkers.begin(),detectedMarkers.end() );
00231     //there might be still the case that a marker is detected twice because of the double border indicated earlier,
00232     //detect and remove these cases
00233     vector<bool> toRemove ( detectedMarkers.size(),false );
00234     for ( int i=0;i<int ( detectedMarkers.size() )-1;++i )
00235     {
00236       if ( detectedMarkers[i].id==detectedMarkers[i+1].id && !toRemove[i+1] )
00237       {
00238         //deletes the one with smaller perimeter
00239         if ( perimeter ( detectedMarkers[i] ) >perimeter ( detectedMarkers[i+1] ) ) toRemove[i+1]=true;
00240         else toRemove[i]=true;
00241       }
00242     }
00243     //remove the markers marker
00244     removeElements ( detectedMarkers, toRemove );
00245 
00247     if ( camMatrix.rows!=0  && markerSizeMeters>0 )
00248     {
00249       for ( unsigned int i=0;i<detectedMarkers.size();i++ )
00250         detectedMarkers[i].calculateExtrinsics ( markerSizeMeters,camMatrix,distCoeff,setYPerpendicular );
00251     }
00252   }
00253 
00254 
00255   /************************************
00256  *
00257  * Crucial step. Detects the rectangular regions of the thresholded image
00258  *
00259  *
00260  ************************************/
00261   void  MarkerDetector::detectRectangles ( const cv::Mat &thres,vector<std::vector<cv::Point2f> > &MarkerCanditates )
00262   {
00263     vector<MarkerCandidate>  candidates;
00264     detectRectangles(thres,candidates);
00265     //create the output
00266     MarkerCanditates.resize(candidates.size());
00267     for (size_t i=0;i<MarkerCanditates.size();i++)
00268       MarkerCanditates[i]=candidates[i];
00269   }
00270 
00271   void MarkerDetector::detectRectangles(const cv::Mat &thresImg,vector<MarkerCandidate> & OutMarkerCanditates)
00272   {
00273     vector<MarkerCandidate>  MarkerCanditates;
00274     //calcualte the min_max contour sizes
00275     unsigned int minSize=_minSize*std::max(thresImg.cols,thresImg.rows)*4;
00276     unsigned int maxSize=_maxSize*std::max(thresImg.cols,thresImg.rows)*4;
00277     std::vector<std::vector<cv::Point> > contours2;
00278     std::vector<cv::Vec4i> hierarchy2;
00279 
00280     thresImg.copyTo ( thres2 );
00281     cv::findContours ( thres2 , contours2, hierarchy2,CV_RETR_TREE, CV_CHAIN_APPROX_NONE );
00282     vector<Point>  approxCurve;
00284 
00285     for ( unsigned int i=0;i<contours2.size();i++ )
00286     {
00287 
00288 
00289       //check it is a possible element by first checking is has enough points
00290       if ( minSize< contours2[i].size() &&contours2[i].size()<maxSize  )
00291       {
00292         //approximate to a poligon
00293         approxPolyDP (  contours2[i]  ,approxCurve , double ( contours2[i].size() ) *0.05 , true );
00294         //                              drawApproxCurve(copy,approxCurve,Scalar(0,0,255));
00295         //check that the poligon has 4 points
00296         if ( approxCurve.size() ==4 )
00297         {
00298 
00299           //       drawContour(input,contours2[i],Scalar(255,0,225));
00300           //              namedWindow("input");
00301           //            imshow("input",input);
00302           //            waitKey(0);
00303           //and is convex
00304           if ( isContourConvex ( Mat ( approxCurve ) ) )
00305           {
00306             //                                        drawApproxCurve(input,approxCurve,Scalar(255,0,255));
00307             //                                          //ensure that the   distace between consecutive points is large enough
00308             float minDist=1e10;
00309             for ( int j=0;j<4;j++ )
00310             {
00311               float d= std::sqrt ( ( float ) ( approxCurve[j].x-approxCurve[ ( j+1 ) %4].x ) * ( approxCurve[j].x-approxCurve[ ( j+1 ) %4].x ) +
00312                                    ( approxCurve[j].y-approxCurve[ ( j+1 ) %4].y ) * ( approxCurve[j].y-approxCurve[ ( j+1 ) %4].y ) );
00313               //                norm(Mat(approxCurve[i]),Mat(approxCurve[(i+1)%4]));
00314               if ( d<minDist ) minDist=d;
00315             }
00316             //check that distance is not very small
00317             if ( minDist>10 )
00318             {
00319               //add the points
00320               //              cout<<"ADDED"<<endl;
00321               MarkerCanditates.push_back ( MarkerCandidate() );
00322               MarkerCanditates.back().idx=i;
00323               for ( int j=0;j<4;j++ )
00324               {
00325                 MarkerCanditates.back().push_back ( Point2f ( approxCurve[j].x,approxCurve[j].y ) );
00326               }
00327             }
00328           }
00329         }
00330       }
00331     }
00332 
00333     //                            namedWindow("input");
00334     //                  imshow("input",input);
00335     //                                                  waitKey(0);
00337     valarray<bool> swapped(false,MarkerCanditates.size());//used later
00338     for ( unsigned int i=0;i<MarkerCanditates.size();i++ )
00339     {
00340 
00341       //trace a line between the first and second point.
00342       //if the thrid point is at the right side, then the points are anti-clockwise
00343       double dx1 = MarkerCanditates[i][1].x - MarkerCanditates[i][0].x;
00344       double dy1 =  MarkerCanditates[i][1].y - MarkerCanditates[i][0].y;
00345       double dx2 = MarkerCanditates[i][2].x - MarkerCanditates[i][0].x;
00346       double dy2 = MarkerCanditates[i][2].y - MarkerCanditates[i][0].y;
00347       double o = ( dx1*dy2 )- ( dy1*dx2 );
00348 
00349       if ( o  < 0.0 )            //if the third point is in the left side, then sort in anti-clockwise order
00350       {
00351         swap ( MarkerCanditates[i][1],MarkerCanditates[i][3] );
00352         swapped[i]=true;
00353         //sort the contour points
00354         //          reverse(MarkerCanditates[i].contour.begin(),MarkerCanditates[i].contour.end());//????
00355 
00356       }
00357     }
00358 
00360     //first detect candidates
00361 
00362     vector<pair<int,int>  > TooNearCandidates;
00363     for ( unsigned int i=0;i<MarkerCanditates.size();i++ )
00364     {
00365       //        cout<<"Marker i="<<i<<MarkerCanditates[i]<<endl;
00366       //calculate the average distance of each corner to the nearest corner of the other marker candidate
00367       for ( unsigned int j=i+1;j<MarkerCanditates.size();j++ )
00368       {
00369         float dist=0;
00370         for ( int c=0;c<4;c++ )
00371           dist+= sqrt ( ( MarkerCanditates[i][c].x-MarkerCanditates[j][c].x ) * ( MarkerCanditates[i][c].x-MarkerCanditates[j][c].x ) + ( MarkerCanditates[i][c].y-MarkerCanditates[j][c].y ) * ( MarkerCanditates[i][c].y-MarkerCanditates[j][c].y ) );
00372         dist/=4;
00373         //if distance is too small
00374         if ( dist< 10 )
00375         {
00376           TooNearCandidates.push_back ( pair<int,int> ( i,j ) );
00377         }
00378       }
00379     }
00380 
00381     //mark for removal the element of  the pair with smaller perimeter
00382     valarray<bool> toRemove ( false,MarkerCanditates.size() );
00383     for ( unsigned int i=0;i<TooNearCandidates.size();i++ )
00384     {
00385       if ( perimeter ( MarkerCanditates[TooNearCandidates[i].first ] ) >perimeter ( MarkerCanditates[ TooNearCandidates[i].second] ) )
00386         toRemove[TooNearCandidates[i].second]=true;
00387       else toRemove[TooNearCandidates[i].first]=true;
00388     }
00389 
00390     //remove the invalid ones
00391     //     removeElements ( MarkerCanditates,toRemove );
00392     //finally, assign to the remaining candidates the contour
00393     OutMarkerCanditates.reserve(MarkerCanditates.size());
00394     for (size_t i=0;i<MarkerCanditates.size();i++) {
00395       if (!toRemove[i]) {
00396         OutMarkerCanditates.push_back(MarkerCanditates[i]);
00397         OutMarkerCanditates.back().contour=contours2[ MarkerCanditates[i].idx];
00398         if (swapped[i] && _enableCylinderWarp )//if the corners where swapped, it is required to reverse here the points so that they are in the same order
00399           reverse(OutMarkerCanditates.back().contour.begin(),OutMarkerCanditates.back().contour.end());//????
00400       }
00401     }
00402 
00403   }
00404 
00405   /************************************
00406  *
00407  *
00408  *
00409  *
00410  ************************************/
00411   void MarkerDetector::thresHold ( int method,const Mat &grey,Mat &out,double param1,double param2 ) throw ( cv::Exception )
00412   {
00413 
00414     if (param1==-1) param1=_thresParam1;
00415     if (param2==-1) param2=_thresParam2;
00416 
00417     if ( grey.type() !=CV_8UC1 )     throw cv::Exception ( 9001,"grey.type()!=CV_8UC1","MarkerDetector::thresHold",__FILE__,__LINE__ );
00418     switch ( method )
00419     {
00420     case FIXED_THRES:
00421       cv::threshold ( grey, out, param1,255, CV_THRESH_BINARY_INV );
00422       break;
00423     case ADPT_THRES://currently, this is the best method
00424       //ensure that _thresParam1%2==1
00425       if ( param1<3 ) param1=3;
00426       else if ( ( ( int ) param1 ) %2 !=1 ) param1= ( int ) ( param1+1 );
00427 
00428       cv::adaptiveThreshold ( grey,out,255,ADAPTIVE_THRESH_MEAN_C,THRESH_BINARY_INV,param1,param2 );
00429       break;
00430     case CANNY:
00431     {
00432       //this should be the best method, and generally it is.
00433       //However, some times there are small holes in the marker contour that makes
00434       //the contour detector not to find it properly
00435       //if there is a missing pixel
00436       cv::Canny ( grey, out, 10, 220 );
00437       //I've tried a closing but it add many more points that some
00438       //times makes this even worse
00439       //                          Mat aux;
00440       //                          cv::morphologyEx(thres,aux,MORPH_CLOSE,Mat());
00441       //                          out=aux;
00442     }
00443       break;
00444     }
00445   }
00446   /************************************
00447  *
00448  *
00449  *
00450  *
00451  ************************************/
00452   bool MarkerDetector::warp ( Mat &in,Mat &out,Size size, vector<Point2f> points ) throw ( cv::Exception )
00453   {
00454 
00455     if ( points.size() !=4 )    throw cv::Exception ( 9001,"point.size()!=4","MarkerDetector::warp",__FILE__,__LINE__ );
00456     //obtain the perspective transform
00457     Point2f  pointsRes[4],pointsIn[4];
00458     for ( int i=0;i<4;i++ ) pointsIn[i]=points[i];
00459     pointsRes[0]= ( Point2f ( 0,0 ) );
00460     pointsRes[1]= Point2f ( size.width-1,0 );
00461     pointsRes[2]= Point2f ( size.width-1,size.height-1 );
00462     pointsRes[3]= Point2f ( 0,size.height-1 );
00463     Mat M=getPerspectiveTransform ( pointsIn,pointsRes );
00464     cv::warpPerspective ( in, out,  M, size,cv::INTER_NEAREST );
00465     return true;
00466   }
00467 
00468   void findCornerPointsInContour(const vector<cv::Point2f>& points,const vector<cv::Point> &contour,vector<int> &idxs)
00469   {
00470     assert(points.size()==4);
00471     int idxSegments[4]={-1,-1,-1,-1};
00472     //the first point coincides with one
00473     cv::Point points2i[4];
00474     for (int i=0;i<4;i++) {
00475       points2i[i].x=points[i].x;
00476       points2i[i].y=points[i].y;
00477     }
00478 
00479     for (size_t i=0;i<contour.size();++i) {
00480       if (idxSegments[0]==-1)
00481         if (contour[i]==points2i[0]) idxSegments[0]=i;
00482       if (idxSegments[1]==-1)
00483         if (contour[i]==points2i[1]) idxSegments[1]=i;
00484       if (idxSegments[2]==-1)
00485         if (contour[i]==points2i[2]) idxSegments[2]=i;
00486       if (idxSegments[3]==-1)
00487         if (contour[i]==points2i[3]) idxSegments[3]=i;
00488     }
00489     idxs.resize(4);
00490     for (int i=0;i<4;i++) idxs[i]=idxSegments[i];
00491   }
00492 
00493   int findDeformedSidesIdx(const vector<cv::Point> &contour,const vector<int> &idxSegments)
00494   {
00495     float distSum[4]={0,0,0,0};
00496 
00497     for (int i=0;i<3;i++) {
00498       cv::Point p1=contour[ idxSegments[i]];
00499       cv::Point p2=contour[ idxSegments[i+1]];
00500       float inv_den=1./ sqrt(float(( p2.x-p1.x)*(p2.x-p1.x)+ (p2.y-p1.y)*(p2.y-p1.y)));
00501       //   d=|v^^·r|=(|(x_2-x_1)(y_1-y_0)-(x_1-x_0)(y_2-y_1)|)/(sqrt((x_2-x_1)^2+(y_2-y_1)^2)).
00502       //         cerr<<"POSS="<<idxSegments[i]<<" "<<idxSegments[i+1]<<endl;
00503       for (int j=idxSegments[i];j<idxSegments[i+1];++j) {
00504         float dist=std::fabs( float(  (p2.x-p1.x)*(p1.y-contour[j].y)-  (p1.x-contour[j].x)*(p2.y-p1.y)) )*inv_den;
00505         distSum[i]+=dist;
00506         //             cerr<< dist<<" ";
00507         //             cv::rectangle(_ssImC,contour[j],contour[j],colors[i],-1);
00508       }
00509       distSum[i]/=float(idxSegments[i+1]-idxSegments[i]);
00510       //         cout<<endl<<endl;
00511     }
00512 
00513 
00514     //for the last one
00515     cv::Point p1=contour[ idxSegments[0]];
00516     cv::Point p2=contour[ idxSegments[3]];
00517     float inv_den=1./ std::sqrt(float(( p2.x-p1.x)*(p2.x-p1.x)+ (p2.y-p1.y)*(p2.y-p1.y)));
00518     //   d=|v^^·r|=(|(x_2-x_1)(y_1-y_0)-(x_1-x_0)(y_2-y_1)|)/(sqrt((x_2-x_1)^2+(y_2-y_1)^2)).
00519     for (int j=0;j<idxSegments[0];++j)
00520       distSum[3]+=std::fabs(   float((p2.x-p1.x)*(p1.y-contour[j].y)-  (p1.x-contour[j].x)*(p2.y-p1.y)))*inv_den;
00521     for (int j=idxSegments[3];j<static_cast<int>(contour.size());++j)
00522       distSum[3]+=std::fabs(   float((p2.x-p1.x)*(p1.y-contour[j].y)-  (p1.x-contour[j].x)*(p2.y-p1.y)))*inv_den;
00523 
00524     distSum[3]/=float(  idxSegments[0]+  (contour.size()-idxSegments[3]));
00525     //now, get the maximum
00526     /*    for (int i=0;i<4;i++)
00527             cout<<"DD="<<distSum[i]<<endl;*/
00528     //check the two combinations to see the one with higher error
00529     if ( distSum[0]+distSum[2]> distSum[1]+distSum[3])
00530       return 0;
00531     else return 1;
00532   }
00533 
00534   void setPointIntoImage(cv::Point2f &p,cv::Size s) {
00535     if (p.x<0) p.x=0;
00536     else if (p.x>=s.width )p.x=s.width-1;
00537     if (p.y<0)p.y=0;
00538     else if (p.y>=s.height)p.y=s.height-1;
00539 
00540   }
00541 
00542   void setPointIntoImage(cv::Point  &p,cv::Size s) {
00543     if (p.x<0) p.x=0;
00544     else if (p.x>=s.width )p.x=s.width-1;
00545     if (p.y<0)p.y=0;
00546     else if (p.y>=s.height)p.y=s.height-1;
00547 
00548   }
00549   /************************************
00550  *
00551  *
00552  *
00553  *
00554  ************************************/
00555   bool MarkerDetector::warp_cylinder ( Mat &in,Mat &out,Size size, MarkerCandidate& mcand ) throw ( cv::Exception )
00556   {
00557 
00558     if ( mcand.size() !=4 )    throw cv::Exception ( 9001,"point.size()!=4","MarkerDetector::warp",__FILE__,__LINE__ );
00559 
00560     //check first the real need for cylinder warping
00561     //     cout<<"im="<<mcand.contour.size()<<endl;
00562 
00563     //     for (size_t i=0;i<mcand.contour.size();i++) {
00564     //         cv::rectangle(_ssImC ,mcand.contour[i],mcand.contour[i],cv::Scalar(111,111,111),-1 );
00565     //     }
00566     //     mcand.draw(imC,cv::Scalar(0,255,0));
00567     //find the 4 different segments of the contour
00568     vector<int> idxSegments;
00569     findCornerPointsInContour(mcand,mcand.contour,idxSegments);
00570     //let us rearrange the points so that the first corner is the one whith smaller idx
00571     int minIdx=0;
00572     for (int i=1;i<4;i++)
00573       if (idxSegments[i] <idxSegments[minIdx]) minIdx=i;
00574     //now, rotate the points to be in this order
00575     std::rotate(idxSegments.begin(),idxSegments.begin()+minIdx,idxSegments.end());
00576     std::rotate(mcand.begin(),mcand.begin()+minIdx,mcand.end());
00577 
00578     //     cout<<"idxSegments="<<idxSegments[0]<< " "<<idxSegments[1]<< " "<<idxSegments[2]<<" "<<idxSegments[3]<<endl;
00579     //now, determine the sides that are deformated by cylinder perspective
00580     int defrmdSide=findDeformedSidesIdx(mcand.contour,idxSegments);
00581     //     cout<<"Def="<<defrmdSide<<endl;
00582 
00583     //instead of removing perspective distortion  of the rectangular region
00584     //given by the rectangle, we enlarge it a bit to include the deformed parts
00585     Point2f enlargedRegion[4];
00586     for (int i=0;i<4;i++) enlargedRegion[i]=mcand[i];
00587     if (defrmdSide==0) {
00588       enlargedRegion[0]=mcand[0]+(mcand[3]-mcand[0])*1.2;
00589       enlargedRegion[1]=mcand[1]+(mcand[2]-mcand[1])*1.2;
00590       enlargedRegion[2]=mcand[2]+(mcand[1]-mcand[2])*1.2;
00591       enlargedRegion[3]=mcand[3]+(mcand[0]-mcand[3])*1.2;
00592     }
00593     else {
00594       enlargedRegion[0]=mcand[0]+(mcand[1]-mcand[0])*1.2;
00595       enlargedRegion[1]=mcand[1]+(mcand[0]-mcand[1])*1.2;
00596       enlargedRegion[2]=mcand[2]+(mcand[3]-mcand[2])*1.2;
00597       enlargedRegion[3]=mcand[3]+(mcand[2]-mcand[3])*1.2;
00598     }
00599     for (size_t i=0;i<4;i++)
00600       setPointIntoImage(enlargedRegion[i],in.size());
00601 
00602     /*
00603         cv::Scalar colors[4]={cv::Scalar(0,0,255),cv::Scalar(255,0,0),cv::Scalar(0,255,0),cv::Scalar(111,111,0)};
00604         for (int i=0;i<4;i++) {
00605             cv::rectangle(_ssImC,mcand.contour[idxSegments[i]]-cv::Point(2,2),mcand.contour[idxSegments[i]]+cv::Point(2,2),colors[i],-1 );
00606             cv::rectangle(_ssImC,enlargedRegion[i]-cv::Point2f(2,2),enlargedRegion[i]+cv::Point2f(2,2),colors[i],-1 );
00607 
00608         }*/
00609     //     cv::imshow("imC",_ssImC);
00610 
00611 
00612     //calculate the max distance from each contour point the line of the corresponding segment it belongs to
00613     //     calculate
00614     //      cv::waitKey(0);
00615     //check that the region is into image limits
00616     //obtain the perspective transform
00617     Point2f  pointsRes[4];
00618 
00619     cv::Size enlargedSize=size;
00620     enlargedSize.width+=2*enlargedSize.width*0.2;
00621     pointsRes[0]= ( Point2f ( 0,0 ) );
00622     pointsRes[1]= Point2f ( enlargedSize.width-1,0 );
00623     pointsRes[2]= Point2f ( enlargedSize.width-1,enlargedSize.height-1 );
00624     pointsRes[3]= Point2f ( 0,enlargedSize.height-1 );
00625     //rotate to ensure that deformed sides are in the horizontal axis when warping
00626     if (defrmdSide==0) rotate(pointsRes,pointsRes+1,pointsRes+4);
00627     cv::Mat imAux,imAux2(enlargedSize,CV_8UC1);
00628     Mat M=getPerspectiveTransform ( enlargedRegion,pointsRes );
00629     cv::warpPerspective ( in, imAux,  M, enlargedSize,cv::INTER_NEAREST);
00630 
00631     //now, transform all points to the new image
00632     vector<cv::Point> pointsCO(mcand.contour.size());
00633     assert(M.type()==CV_64F);
00634     assert(M.cols==3 && M.rows==3);
00635     //     cout<<M<<endl;
00636     double *mptr=M.ptr<double>(0);
00637     imAux2.setTo(cv::Scalar::all(0));
00638 
00639 
00640     for (size_t i=0;i<mcand.contour.size();i++) {
00641       float inX=mcand.contour[i].x;
00642       float inY=mcand.contour[i].y;
00643       float w= inX * mptr[6]+inY * mptr[7]+mptr[8];
00644       cv::Point2f pres;
00645       pointsCO[i].x=( (inX * mptr[0]+inY* mptr[1]+mptr[2])/w)+0.5;
00646       pointsCO[i].y=( (inX * mptr[3]+inY* mptr[4]+mptr[5])/w)+0.5;
00647       //make integers
00648       setPointIntoImage(pointsCO[i],imAux.size());//ensure points are into image limits
00649       //        cout<<"p="<<pointsCO[i]<<" "<<imAux.size().width<<" "<<imAux.size().height<<endl;
00650       imAux2.at<uchar>(pointsCO[i].y,pointsCO[i].x)=255;
00651       if (pointsCO[i].y>0)
00652         imAux2.at<uchar>(pointsCO[i].y-1,pointsCO[i].x)=255;
00653       if (pointsCO[i].y<imAux2.rows-1 )
00654         imAux2.at<uchar>(pointsCO[i].y+1,pointsCO[i].x)=255;
00655     }
00656 
00657     cv::Mat outIm(enlargedSize,CV_8UC1);
00658     outIm.setTo(cv::Scalar::all(0));
00659     //now, scan in lines to determine the required displacement
00660     for (int y=0;y<imAux2.rows;y++) {
00661       uchar *_offInfo=imAux2.ptr<uchar>(y);
00662       int start=-1,end=-1;
00663       //determine the start and end of markerd regions
00664       for (int x=0;x<imAux.cols;x++) {
00665         if (_offInfo[x]) {
00666           if (start==-1) start=x;
00667           else end=x;
00668         }
00669       }
00670       //       cout<<"S="<<start<<" "<<end<<" "<<end-start<<" "<<(size.width>>1)<<endl;
00671       //check that the size is big enough and
00672       assert(start!=-1 && end!=-1 && (end-start)> size.width>>1);
00673       uchar *In_image=imAux.ptr<uchar>(y);
00674       uchar *Out_image=outIm.ptr<uchar>(y);
00675       memcpy(Out_image,In_image+start,imAux.cols-start );
00676     }
00677 
00678 
00679     //     cout<<"SS="<<mcand.contour.size()<<" "<<pointsCO.size()<<endl;
00680     //get the central region with the size specified
00681     cv::Mat centerReg=outIm(cv::Range::all(),cv::Range(0,size.width));
00682     out=centerReg.clone();
00683     //     cv::perspectiveTransform(mcand.contour,pointsCO,M);
00684     //draw them
00685     //     cv::imshow("out2",out);
00686     //     cv::imshow("imm",imAux2);
00687     //     cv::waitKey(0);
00688     return true;
00689   }
00690   /************************************
00691  *
00692  *
00693  *
00694  *
00695  ************************************/
00696   bool MarkerDetector::isInto ( Mat &contour,vector<Point2f> &b )
00697   {
00698 
00699     for ( size_t i=0;i<b.size();i++ )
00700       if ( pointPolygonTest ( contour,b[i],false ) >0 ) return true;
00701     return false;
00702   }
00703   /************************************
00704  *
00705  *
00706  *
00707  *
00708  ************************************/
00709   int MarkerDetector:: perimeter ( vector<Point2f> &a )
00710   {
00711     int sum=0;
00712     for ( size_t i=0;i<a.size();i++ )
00713     {
00714       int i2= ( i+1 ) %a.size();
00715       sum+= sqrt ( ( a[i].x-a[i2].x ) * ( a[i].x-a[i2].x ) + ( a[i].y-a[i2].y ) * ( a[i].y-a[i2].y ) ) ;
00716     }
00717     return sum;
00718   }
00719 
00720 
00725   void MarkerDetector::findBestCornerInRegion_harris ( const cv::Mat  & grey,vector<cv::Point2f> &  Corners,int blockSize )
00726   {
00727     int halfSize=blockSize/2;
00728     for ( size_t i=0;i<Corners.size();i++ )
00729     {
00730       //check that the region is into the image limits
00731       cv::Point2f min ( Corners[i].x-halfSize,Corners[i].y-halfSize );
00732       cv::Point2f max ( Corners[i].x+halfSize,Corners[i].y+halfSize );
00733       if ( min.x>=0  &&  min.y>=0 && max.x<grey.cols && max.y<grey.rows )
00734       {
00735         cv::Mat response;
00736         cv::Mat subImage ( grey,cv::Rect ( Corners[i].x-halfSize,Corners[i].y-halfSize,blockSize ,blockSize ) );
00737         vector<Point2f> corners2;
00738         goodFeaturesToTrack ( subImage, corners2, 10, 0.001, halfSize );
00739         float minD=9999;
00740         int bIdx=-1;
00741         cv::Point2f Center ( halfSize,halfSize );
00742         for ( size_t j=0;j<corners2.size();j++ )
00743         {
00744           float dist=cv::norm ( corners2[j]-Center );
00745           if ( dist<minD )
00746           {
00747             minD=dist;
00748             bIdx=j;
00749           }
00750           if ( minD<halfSize ) Corners[i]+= ( corners2[bIdx]-Center );
00751         }
00752       }
00753     }
00754   }
00755 
00756 
00761   void MarkerDetector::refineCandidateLines(MarkerDetector::MarkerCandidate& candidate)
00762   {
00763     // search corners on the contour vector
00764     vector<size_t> cornerIndex;
00765     cornerIndex.resize(4);
00766     for(size_t j=0; j<candidate.contour.size(); ++j) {
00767       for(size_t k=0; k<4; ++k) {
00768         if(candidate.contour[j].x==candidate[k].x && candidate.contour[j].y==candidate[k].y) {
00769           cornerIndex[k] = j;
00770         }
00771       }
00772     }
00773 
00774     // contour pixel in inverse order or not?
00775     bool inverse;
00776     if( (cornerIndex[1] > cornerIndex[0]) && (cornerIndex[2]>cornerIndex[1] || cornerIndex[2]<cornerIndex[0]) )
00777       inverse = false;
00778     else if( cornerIndex[2]>cornerIndex[1] && cornerIndex[2]<cornerIndex[0] )
00779       inverse = false;
00780     else inverse = true;
00781 
00782 
00783     // get pixel vector for each line of the marker
00784     int inc = 1;
00785     if(inverse) inc = -1;
00786 
00787     vector<std::vector<cv::Point> > contourLines;
00788     contourLines.resize(4);
00789     for(unsigned int l=0; l<4; ++l) {
00790       for(int j=(int)cornerIndex[l]; j!=(int)cornerIndex[(l+1)%4]; j+=inc) {
00791         if(j==(int)candidate.contour.size() && !inverse) j=0;
00792         else if(j==0 && inverse) j=candidate.contour.size()-1;
00793         contourLines[l].push_back(candidate.contour[j]);
00794         if(j==(int)cornerIndex[(l+1)%4]) break; // this has to be added because of the previous ifs
00795       }
00796 
00797     }
00798 
00799     // interpolate marker lines
00800     vector<Point3f> lines;
00801     lines.resize(4);
00802     for(unsigned int j=0; j<lines.size(); ++j)
00803       interpolate2Dline(contourLines[j], lines[j]);
00804 
00805     // get cross points of lines
00806     vector<Point2f> crossPoints;
00807     crossPoints.resize(4);
00808     for(unsigned int i=0; i<4; ++i)
00809       crossPoints[i] = getCrossPoint( lines[(i-1)%4], lines[i] );
00810 
00811     // reassing points
00812     for(unsigned int j=0; j<4; ++j)
00813       candidate[j] = crossPoints[j];
00814   }
00815 
00816 
00819   void MarkerDetector::interpolate2Dline( const std::vector< Point >& inPoints, Point3f& outLine)
00820   {
00821 
00822     float minX, maxX, minY, maxY;
00823     minX = maxX = inPoints[0].x;
00824     minY = maxY = inPoints[0].y;
00825     for(unsigned int i=1; i<inPoints.size(); ++i)  {
00826       if(inPoints[i].x < minX) minX = inPoints[i].x;
00827       if(inPoints[i].x > maxX) maxX = inPoints[i].x;
00828       if(inPoints[i].y < minY) minY = inPoints[i].y;
00829       if(inPoints[i].y > maxY) maxY = inPoints[i].y;
00830     }
00831 
00832     // create matrices of equation system
00833     Mat A(inPoints.size(),2,CV_32FC1, Scalar(0));
00834     Mat B(inPoints.size(),1,CV_32FC1, Scalar(0));
00835     Mat X;
00836 
00837     
00838     
00839     if( maxX-minX > maxY-minY ) {
00840       // Ax + C = y
00841       for (size_t i=0; i<inPoints.size(); ++i) {
00842 
00843         A.at<float>(i, 0) = inPoints[i].x;
00844         A.at<float>(i, 1) = 1.;
00845         B.at<float>(i, 0) = inPoints[i].y;
00846 
00847       }
00848 
00849       // solve system
00850       solve(A,B,X, DECOMP_SVD);
00851       // return Ax + By + C
00852       outLine = Point3f(X.at<float>(0,0), -1., X.at<float>(1,0));
00853     }
00854     else {
00855       // By + C = x
00856       for (size_t i=0; i<inPoints.size(); ++i) {
00857 
00858         A.at<float>(i, 0) = inPoints[i].y;
00859         A.at<float>(i, 1) = 1.;
00860         B.at<float>(i, 0) = inPoints[i].x;
00861 
00862       }
00863 
00864       // solve system
00865       solve(A,B,X, DECOMP_SVD);
00866       // return Ax + By + C
00867       outLine = Point3f(-1., X.at<float>(0,0), X.at<float>(1,0));
00868     }
00869 
00870   }
00871 
00874   Point2f MarkerDetector::getCrossPoint(const cv::Point3f& line1, const cv::Point3f& line2)
00875   {
00876 
00877     // create matrices of equation system
00878     Mat A(2,2,CV_32FC1, Scalar(0));
00879     Mat B(2,1,CV_32FC1, Scalar(0));
00880     Mat X;
00881 
00882     A.at<float>(0, 0) = line1.x;
00883     A.at<float>(0, 1) = line1.y;
00884     B.at<float>(0, 0) = -line1.z;
00885 
00886     A.at<float>(1, 0) = line2.x;
00887     A.at<float>(1, 1) = line2.y;
00888     B.at<float>(1, 0) = -line2.z;
00889 
00890     // solve system
00891     solve(A,B,X, DECOMP_SVD);
00892     return Point2f(X.at<float>(0,0), X.at<float>(1,0));
00893 
00894   }
00895 
00896 
00897 
00898 
00899 
00900 
00901 
00902 
00903   /************************************
00904  *
00905  *
00906  *
00907  *
00908  ************************************/
00909   void MarkerDetector::drawAllContours ( Mat input, std::vector<std::vector<cv::Point> > &contours )
00910   {
00911     drawContours ( input,  contours, -1,Scalar ( 255,0,255 ) );
00912   }
00913 
00914   /************************************
00915  *
00916  *
00917  *
00918  *
00919  ************************************/
00920   void MarkerDetector:: drawContour ( Mat &in,vector<Point>  &contour,Scalar color )
00921   {
00922     for ( size_t i=0;i<contour.size();++i )
00923     {
00924       cv::rectangle ( in,contour[i],contour[i],color );
00925     }
00926   }
00927 
00928   void  MarkerDetector:: drawApproxCurve ( Mat &in,vector<Point>  &contour,Scalar color )
00929   {
00930     for ( size_t i=0;i<contour.size();++i )
00931     {
00932       cv::line ( in,contour[i],contour[ ( i+1 ) %contour.size() ],color );
00933     }
00934   }
00935   /************************************
00936  *
00937  *
00938  *
00939  *
00940  ************************************/
00941 
00942   void MarkerDetector::draw ( Mat out,const vector<Marker> &markers )
00943   {
00944     for ( size_t i=0;i<markers.size();++i )
00945     {
00946       cv::line ( out,markers[i][0],markers[i][1],cvScalar ( 255,0,0 ),2,CV_AA );
00947       cv::line ( out,markers[i][1],markers[i][2],cvScalar ( 255,0,0 ),2,CV_AA );
00948       cv::line ( out,markers[i][2],markers[i][3],cvScalar ( 255,0,0 ),2,CV_AA );
00949       cv::line ( out,markers[i][3],markers[i][0],cvScalar ( 255,0,0 ),2,CV_AA );
00950     }
00951   }
00952   /* Attempt to make it faster than in opencv. I could not :( Maybe trying with SSE3...
00953 void MarkerDetector::warpPerspective(const cv::Mat &in,cv::Mat & out, const cv::Mat & M,cv::Size size)
00954 {
00955    //inverse the matrix
00956    out.create(size,in.type());
00957    //convert to float to speed up operations
00958    const double *m=M.ptr<double>(0);
00959    float mf[9];
00960    mf[0]=m[0];mf[1]=m[1];mf[2]=m[2];
00961    mf[3]=m[3];mf[4]=m[4];mf[5]=m[5];
00962    mf[6]=m[6];mf[7]=m[7];mf[8]=m[8];
00963 
00964    for(int y=0;y<out.rows;y++){
00965      uchar *_ptrout=out.ptr<uchar>(y);
00966      for(int x=0;x<out.cols;x++){
00967    //get the x,y position
00968    float den=1./(x*mf[6]+y*mf[7]+mf[8]);
00969    float ox= (x*mf[0]+y*mf[1]+mf[2])*den;
00970    float oy= (x*mf[3]+y*mf[4]+mf[5])*den;
00971    _ptrout[x]=in.at<uchar>(oy,ox);
00972      }
00973    }
00974 }
00975 */
00976 
00977   /************************************
00978  *
00979  *
00980  *
00981  *
00982  ************************************/
00983 
00984   void MarkerDetector::glGetProjectionMatrix ( CameraParameters &  CamMatrix,cv::Size orgImgSize, cv::Size size,double proj_matrix[16],double gnear,double gfar,bool invert )
00985   throw ( cv::Exception )
00986   {
00987     cerr<<"MarkerDetector::glGetProjectionMatrix . This a deprecated function. Use CameraParameters::glGetProjectionMatrix instead. "<<__FILE__<<" "<<__LINE__<<endl;
00988     CamMatrix.glGetProjectionMatrix ( orgImgSize,size,proj_matrix,gnear,gfar,invert );
00989   }
00990 
00991   /************************************
00992 *
00993 *
00994 *
00995 *
00996 ************************************/
00997 
00998   void MarkerDetector::setMinMaxSize(float min ,float max )throw(cv::Exception)
00999   {
01000     if (min<=0 || min>1) throw cv::Exception(1," min parameter out of range","MarkerDetector::setMinMaxSize",__FILE__,__LINE__);
01001     if (max<=0 || max>1) throw cv::Exception(1," max parameter out of range","MarkerDetector::setMinMaxSize",__FILE__,__LINE__);
01002     if (min>max) throw cv::Exception(1," min>max","MarkerDetector::setMinMaxSize",__FILE__,__LINE__);
01003     _minSize=min;
01004     _maxSize=max;
01005   }
01006 
01007 }
01008 


aruco
Author(s): Rafael Muñoz Salinas , Bence Magyar
autogenerated on Wed Jul 26 2017 02:17:20