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


ar_sys
Author(s): Hamdi Sahloul , Rafael Muñoz Salinas , Bence Magyar
autogenerated on Sun Oct 5 2014 22:12:49