00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028 #include "markerdetector.h"
00029 #include <opencv/cv.h>
00030 #include <opencv/highgui.h>
00031 #include <iostream>
00032 #include <fstream>
00033 #include "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;
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 setYPerperdicular) throw ( cv::Exception )
00108 {
00109 detect ( input, detectedMarkers,camParams.CameraMatrix ,camParams.Distorsion, markerSizeMeters ,setYPerperdicular);
00110 }
00111
00112
00113
00114 void MarkerDetector::detect2 ( const cv::Mat &input,std::vector<Marker> &detectedMarkers, CameraParameters camParams ,vector<float> markerSizeMeters, std::vector<cv::Point3f> markerPositionMeters ,bool setYPerperdicular) throw ( cv::Exception )
00115 {
00116 detect2 ( input, detectedMarkers,camParams.CameraMatrix ,camParams.Distorsion, markerSizeMeters, markerPositionMeters ,setYPerperdicular);
00117 }
00118
00119
00120
00121
00122
00123
00124 void MarkerDetector::detect ( const cv::Mat &input,vector<Marker> &detectedMarkers,Mat camMatrix ,Mat distCoeff ,float markerSizeMeters ,bool setYPerperdicular) throw ( cv::Exception )
00125 {
00126
00127
00128
00129 if ( input.type() ==CV_8UC3 ) cv::cvtColor ( input,grey,CV_BGR2GRAY );
00130 else grey=input;
00131
00132
00133
00134
00135 detectedMarkers.clear();
00136
00137
00138 cv::Mat imgToBeThresHolded=grey;
00139 double ThresParam1=_thresParam1,ThresParam2=_thresParam2;
00140
00141 if ( pyrdown_level!=0 )
00142 {
00143 reduced=grey;
00144 for ( int i=0;i<pyrdown_level;i++ )
00145 {
00146 cv::Mat tmp;
00147 cv::pyrDown ( reduced,tmp );
00148 reduced=tmp;
00149 }
00150 int red_den=pow ( 2.0f,pyrdown_level );
00151 imgToBeThresHolded=reduced;
00152 ThresParam1/=float ( red_den );
00153 ThresParam2/=float ( red_den );
00154 }
00155
00157 thresHold ( _thresMethod,imgToBeThresHolded,thres,ThresParam1,ThresParam2 );
00158
00159
00160 if ( _doErosion )
00161 {
00162 erode ( thres,thres2,cv::Mat() );
00163 thres2=thres;
00164
00165 }
00166
00167 vector<MarkerCandidate > MarkerCanditates;
00168 detectRectangles ( thres,MarkerCanditates );
00169
00170 if ( pyrdown_level!=0 )
00171 {
00172 float red_den=pow ( 2.0f,pyrdown_level );
00173 float offInc= ( ( pyrdown_level/2. )-0.5 );
00174 for ( unsigned int i=0;i<MarkerCanditates.size();i++ ) {
00175 for ( int c=0;c<4;c++ )
00176 {
00177 MarkerCanditates[i][c].x=MarkerCanditates[i][c].x*red_den+offInc;
00178 MarkerCanditates[i][c].y=MarkerCanditates[i][c].y*red_den+offInc;
00179 }
00180
00181 for ( int c=0;c<MarkerCanditates[i].contour.size();c++ )
00182 {
00183 MarkerCanditates[i].contour[c].x=MarkerCanditates[i].contour[c].x*red_den+offInc;
00184 MarkerCanditates[i].contour[c].y=MarkerCanditates[i].contour[c].y*red_den+offInc;
00185 }
00186 }
00187 }
00188
00190 _candidates.clear();
00191 for ( unsigned int i=0;i<MarkerCanditates.size();i++ )
00192 {
00193
00194 Mat canonicalMarker;
00195 bool resW=false;
00196 if (_enableCylinderWarp)
00197 resW=warp_cylinder( grey,canonicalMarker,Size ( _markerWarpSize,_markerWarpSize ),MarkerCanditates[i] );
00198 else resW=warp ( grey,canonicalMarker,Size ( _markerWarpSize,_markerWarpSize ),MarkerCanditates[i] );
00199 if (resW) {
00200 int nRotations;
00201 int id= ( *markerIdDetector_ptrfunc ) ( canonicalMarker,nRotations );
00202 if ( id!=-1 )
00203 {
00204 if(_cornerMethod==LINES) refineCandidateLines( MarkerCanditates[i] );
00205 detectedMarkers.push_back ( MarkerCanditates[i] );
00206 detectedMarkers.back().id=id;
00207
00208 std::rotate ( detectedMarkers.back().begin(),detectedMarkers.back().begin() +4-nRotations,detectedMarkers.back().end() );
00209 }
00210 else _candidates.push_back ( MarkerCanditates[i] );
00211 }
00212
00213 }
00214
00215
00217 if ( detectedMarkers.size() >0 && _cornerMethod!=NONE && _cornerMethod!=LINES )
00218 {
00219 vector<Point2f> Corners;
00220 for ( unsigned int i=0;i<detectedMarkers.size();i++ )
00221 for ( int c=0;c<4;c++ )
00222 Corners.push_back ( detectedMarkers[i][c] );
00223
00224 if ( _cornerMethod==HARRIS )
00225 findBestCornerInRegion_harris ( grey, Corners,7 );
00226 else if ( _cornerMethod==SUBPIX )
00227 cornerSubPix ( grey, Corners,cvSize ( 5,5 ), cvSize ( -1,-1 ) ,cvTermCriteria ( CV_TERMCRIT_ITER|CV_TERMCRIT_EPS,3,0.05 ) );
00228
00229
00230 for ( unsigned int i=0;i<detectedMarkers.size();i++ )
00231 for ( int c=0;c<4;c++ ) detectedMarkers[i][c]=Corners[i*4+c];
00232 }
00233
00234
00235 std::sort ( detectedMarkers.begin(),detectedMarkers.end() );
00236
00237
00238 vector<bool> toRemove ( detectedMarkers.size(),false );
00239
00240
00241 for ( int i=0;i<int ( detectedMarkers.size() )-1;i++ )
00242 {
00243 if ( detectedMarkers[i].id==detectedMarkers[i+1].id && !toRemove[i+1] )
00244 {
00245
00246 if ( perimeter ( detectedMarkers[i] ) >perimeter ( detectedMarkers[i+1] ) ) toRemove[i+1]=true;
00247 else toRemove[i]=true;
00248 }
00249 }
00250
00251 removeElements ( detectedMarkers, toRemove );
00252
00254 if ( camMatrix.rows!=0 && markerSizeMeters>0 )
00255 {
00256 for ( unsigned int i=0;i<detectedMarkers.size();i++ )
00257 detectedMarkers[i].calculateExtrinsics ( markerSizeMeters,camMatrix,distCoeff,setYPerperdicular );
00258 }
00259
00260
00261
00262 for ( unsigned int i=0;i<detectedMarkers.size();i++ ){
00263
00264 Point cent(0,0);
00265 for (int j=0;j<4;j++)
00266 {
00267 cent.x+=detectedMarkers[i][j].x;
00268 cent.y+=detectedMarkers[i][j].y;
00269 }
00270 cent.x/=4.;
00271 cent.y/=4.;
00272 detectedMarkers[i].marker_center_img=cent;
00273 }
00274
00275 }
00276
00277
00278 void MarkerDetector::detect2 (
00279 const cv::Mat &input,
00280 vector<Marker> &detectedMarkers,
00281 Mat camMatrix ,
00282 Mat distCoeff ,
00283 std::vector<float> markerSizeMeters,
00284 std::vector<cv::Point3f> markerPositionMeters,
00285 bool setYPerperdicular) throw ( cv::Exception )
00286 {
00287
00288
00289
00290 if ( input.type() ==CV_8UC3 ) cv::cvtColor ( input,grey,CV_BGR2GRAY );
00291 else grey=input;
00292
00293
00294
00295
00296 detectedMarkers.clear();
00297
00298
00299 cv::Mat imgToBeThresHolded=grey;
00300 double ThresParam1=_thresParam1,ThresParam2=_thresParam2;
00301
00302 if ( pyrdown_level!=0 )
00303 {
00304 reduced=grey;
00305 for ( int i=0;i<pyrdown_level;i++ )
00306 {
00307 cv::Mat tmp;
00308 cv::pyrDown ( reduced,tmp );
00309 reduced=tmp;
00310 }
00311 int red_den=pow ( 2.0f,pyrdown_level );
00312 imgToBeThresHolded=reduced;
00313 ThresParam1/=float ( red_den );
00314 ThresParam2/=float ( red_den );
00315 }
00316
00318 thresHold ( _thresMethod,imgToBeThresHolded,thres,ThresParam1,ThresParam2 );
00319
00320
00321 if ( _doErosion )
00322 {
00323 erode ( thres,thres2,cv::Mat() );
00324 thres2=thres;
00325
00326 }
00327
00328 vector<MarkerCandidate > MarkerCanditates;
00329 detectRectangles ( thres,MarkerCanditates );
00330
00331 if ( pyrdown_level!=0 )
00332 {
00333 float red_den=pow ( 2.0f,pyrdown_level );
00334 float offInc= ( ( pyrdown_level/2. )-0.5 );
00335 for ( unsigned int i=0;i<MarkerCanditates.size();i++ ) {
00336 for ( int c=0;c<4;c++ )
00337 {
00338 MarkerCanditates[i][c].x=MarkerCanditates[i][c].x*red_den+offInc;
00339 MarkerCanditates[i][c].y=MarkerCanditates[i][c].y*red_den+offInc;
00340 }
00341
00342 for ( int c=0;c<MarkerCanditates[i].contour.size();c++ )
00343 {
00344 MarkerCanditates[i].contour[c].x=MarkerCanditates[i].contour[c].x*red_den+offInc;
00345 MarkerCanditates[i].contour[c].y=MarkerCanditates[i].contour[c].y*red_den+offInc;
00346 }
00347 }
00348 }
00349
00351 _candidates.clear();
00352 for ( unsigned int i=0;i<MarkerCanditates.size();i++ )
00353 {
00354
00355 Mat canonicalMarker;
00356 bool resW=false;
00357 if (_enableCylinderWarp)
00358 resW=warp_cylinder( grey,canonicalMarker,Size ( _markerWarpSize,_markerWarpSize ),MarkerCanditates[i] );
00359 else resW=warp ( grey,canonicalMarker,Size ( _markerWarpSize,_markerWarpSize ),MarkerCanditates[i] );
00360 if (resW) {
00361 int nRotations;
00362 int id= ( *markerIdDetector_ptrfunc ) ( canonicalMarker,nRotations );
00363 if ( id!=-1 )
00364 {
00365 if(_cornerMethod==LINES) refineCandidateLines( MarkerCanditates[i] );
00366 detectedMarkers.push_back ( MarkerCanditates[i] );
00367 detectedMarkers.back().id=id;
00368
00369 std::rotate ( detectedMarkers.back().begin(),detectedMarkers.back().begin() +4-nRotations,detectedMarkers.back().end() );
00370 }
00371 else _candidates.push_back ( MarkerCanditates[i] );
00372 }
00373
00374 }
00375
00376
00378 if ( detectedMarkers.size() >0 && _cornerMethod!=NONE && _cornerMethod!=LINES )
00379 {
00380 vector<Point2f> Corners;
00381 for ( unsigned int i=0;i<detectedMarkers.size();i++ )
00382 for ( int c=0;c<4;c++ )
00383 Corners.push_back ( detectedMarkers[i][c] );
00384
00385 if ( _cornerMethod==HARRIS )
00386 findBestCornerInRegion_harris ( grey, Corners,7 );
00387 else if ( _cornerMethod==SUBPIX )
00388 cornerSubPix ( grey, Corners,cvSize ( 5,5 ), cvSize ( -1,-1 ) ,cvTermCriteria ( CV_TERMCRIT_ITER|CV_TERMCRIT_EPS,3,0.05 ) );
00389
00390
00391 for ( unsigned int i=0;i<detectedMarkers.size();i++ )
00392 for ( int c=0;c<4;c++ ) detectedMarkers[i][c]=Corners[i*4+c];
00393 }
00394
00395
00396 std::sort ( detectedMarkers.begin(),detectedMarkers.end() );
00397
00398
00399 vector<bool> toRemove ( detectedMarkers.size(),false );
00400
00401
00402 for ( int i=0;i<int ( detectedMarkers.size() )-1;i++ )
00403 {
00404 if ( detectedMarkers[i].id==detectedMarkers[i+1].id && !toRemove[i+1] )
00405 {
00406
00407 if ( perimeter ( detectedMarkers[i] ) >perimeter ( detectedMarkers[i+1] ) ) toRemove[i+1]=true;
00408 else toRemove[i]=true;
00409 }
00410 }
00411
00412 removeElements ( detectedMarkers, toRemove );
00413
00415 if ( camMatrix.rows!=0 )
00416 {
00417 for ( unsigned int i=0;i<detectedMarkers.size();i++ )
00418 if(markerSizeMeters[detectedMarkers[i].id] != -1){
00419 detectedMarkers[i].calculateExtrinsics ( markerSizeMeters[detectedMarkers[i].id],camMatrix,distCoeff,setYPerperdicular );
00420
00421
00422
00423
00424
00425
00426 }
00427 }
00428
00429
00430 for ( unsigned int i=0;i<detectedMarkers.size();i++ ){
00431
00432 Point cent(0,0);
00433 for (int j=0;j<4;j++)
00434 {
00435 cent.x+=detectedMarkers[i][j].x;
00436 cent.y+=detectedMarkers[i][j].y;
00437 }
00438 cent.x/=4.;
00439 cent.y/=4.;
00440 detectedMarkers[i].marker_center_img=cent;
00441 }
00442
00443 }
00444
00445
00446
00448 void MarkerDetector::detect_diffs ( const cv::Mat &input,vector<Marker> &detectedMarkers,Mat camMatrix ,Mat distCoeff ,float markerSizeMeters ,bool setYPerperdicular) throw ( cv::Exception )
00449 {
00450
00451
00452
00453 if ( input.type() ==CV_8UC3 ) cv::cvtColor ( input,grey,CV_BGR2GRAY );
00454 else grey=input;
00455
00456
00457
00458
00459
00460 detectedMarkers.clear();
00461
00462
00463 cv::Mat imgToBeThresHolded=grey;
00464 double ThresParam1=_thresParam1,ThresParam2=_thresParam2;
00465
00466 if ( pyrdown_level!=0 )
00467 {
00468 reduced=grey;
00469 for ( int i=0;i<pyrdown_level;i++ )
00470 {
00471 cv::Mat tmp;
00472 cv::pyrDown ( reduced,tmp );
00473 reduced=tmp;
00474 }
00475 int red_den=pow ( 2.0f,pyrdown_level );
00476 imgToBeThresHolded=reduced;
00477 ThresParam1/=float ( red_den );
00478 ThresParam2/=float ( red_den );
00479 }
00480
00482 thresHold ( _thresMethod,imgToBeThresHolded,thres,ThresParam1,ThresParam2 );
00483
00484
00485 if ( _doErosion )
00486 {
00487 erode ( thres,thres2,cv::Mat() );
00488 thres2=thres;
00489
00490 }
00491
00492 vector<MarkerCandidate > MarkerCanditates;
00493 detectRectangles ( thres,MarkerCanditates );
00494
00495 if ( pyrdown_level!=0 )
00496 {
00497 float red_den=pow ( 2.0f,pyrdown_level );
00498 float offInc= ( ( pyrdown_level/2. )-0.5 );
00499 for ( unsigned int i=0;i<MarkerCanditates.size();i++ ) {
00500 for ( int c=0;c<4;c++ )
00501 {
00502 MarkerCanditates[i][c].x=MarkerCanditates[i][c].x*red_den+offInc;
00503 MarkerCanditates[i][c].y=MarkerCanditates[i][c].y*red_den+offInc;
00504 }
00505
00506 for ( int c=0;c<MarkerCanditates[i].contour.size();c++ )
00507 {
00508 MarkerCanditates[i].contour[c].x=MarkerCanditates[i].contour[c].x*red_den+offInc;
00509 MarkerCanditates[i].contour[c].y=MarkerCanditates[i].contour[c].y*red_den+offInc;
00510 }
00511 }
00512 }
00513
00515 _candidates.clear();
00516 for ( unsigned int i=0;i<MarkerCanditates.size();i++ )
00517 {
00518
00519 Mat canonicalMarker;
00520 bool resW=false;
00521 if (_enableCylinderWarp)
00522 resW=warp_cylinder( grey,canonicalMarker,Size ( _markerWarpSize,_markerWarpSize ),MarkerCanditates[i] );
00523 else resW=warp ( grey,canonicalMarker,Size ( _markerWarpSize,_markerWarpSize ),MarkerCanditates[i] );
00524 if (resW) {
00525 int nRotations;
00526 int id= ( *markerIdDetector_ptrfunc ) ( canonicalMarker,nRotations );
00527 if ( id!=-1 )
00528 {
00529 if(_cornerMethod==LINES) refineCandidateLines( MarkerCanditates[i] );
00530 detectedMarkers.push_back ( MarkerCanditates[i] );
00531 detectedMarkers.back().id=id;
00532
00533 std::rotate ( detectedMarkers.back().begin(),detectedMarkers.back().begin() +4-nRotations,detectedMarkers.back().end() );
00534 }
00535 else _candidates.push_back ( MarkerCanditates[i] );
00536 }
00537
00538 }
00539
00540
00542 if ( detectedMarkers.size() >0 && _cornerMethod!=NONE && _cornerMethod!=LINES )
00543 {
00544 vector<Point2f> Corners;
00545 for ( unsigned int i=0;i<detectedMarkers.size();i++ )
00546 for ( int c=0;c<4;c++ )
00547 Corners.push_back ( detectedMarkers[i][c] );
00548
00549 if ( _cornerMethod==HARRIS )
00550 findBestCornerInRegion_harris ( grey, Corners,7 );
00551 else if ( _cornerMethod==SUBPIX )
00552 cornerSubPix ( grey, Corners,cvSize ( 5,5 ), cvSize ( -1,-1 ) ,cvTermCriteria ( CV_TERMCRIT_ITER|CV_TERMCRIT_EPS,3,0.05 ) );
00553
00554
00555 for ( unsigned int i=0;i<detectedMarkers.size();i++ )
00556 for ( int c=0;c<4;c++ ) detectedMarkers[i][c]=Corners[i*4+c];
00557 }
00558
00559 std::sort ( detectedMarkers.begin(),detectedMarkers.end() );
00560
00561
00562 vector<bool> toRemove ( detectedMarkers.size(),false );
00563 for ( int i=0;i<int ( detectedMarkers.size() )-1;i++ )
00564 {
00565 if ( detectedMarkers[i].id==detectedMarkers[i+1].id && !toRemove[i+1] )
00566 {
00567
00568 if ( perimeter ( detectedMarkers[i] ) >perimeter ( detectedMarkers[i+1] ) ) toRemove[i+1]=true;
00569 else toRemove[i]=true;
00570 }
00571 }
00572
00573 removeElements ( detectedMarkers, toRemove );
00574
00576 if ( camMatrix.rows!=0 && markerSizeMeters>0 )
00577 {
00578 for ( unsigned int i=0;i<detectedMarkers.size();i++ )
00579 detectedMarkers[i].calculateExtrinsics ( markerSizeMeters,camMatrix,distCoeff,setYPerperdicular );
00580 }
00581 }
00582
00583
00585
00586
00587
00588
00589
00590
00591
00592 void MarkerDetector::detectRectangles ( const cv::Mat &thres,vector<std::vector<cv::Point2f> > &MarkerCanditates )
00593 {
00594 vector<MarkerCandidate> candidates;
00595 detectRectangles(thres,candidates);
00596
00597 MarkerCanditates.resize(candidates.size());
00598 for (size_t i=0;i<MarkerCanditates.size();i++)
00599 MarkerCanditates[i]=candidates[i];
00600 }
00601
00602 void MarkerDetector::detectRectangles(const cv::Mat &thresImg,vector<MarkerCandidate> & OutMarkerCanditates)
00603 {
00604 vector<MarkerCandidate> MarkerCanditates;
00605
00606 int minSize=_minSize*std::max(thresImg.cols,thresImg.rows)*4;
00607 int maxSize=_maxSize*std::max(thresImg.cols,thresImg.rows)*4;
00608 std::vector<std::vector<cv::Point> > contours2;
00609 std::vector<cv::Vec4i> hierarchy2;
00610
00611 thresImg.copyTo ( thres2 );
00612 cv::findContours ( thres2 , contours2, hierarchy2,CV_RETR_TREE, CV_CHAIN_APPROX_NONE );
00613 vector<Point> approxCurve;
00615
00616 for ( unsigned int i=0;i<contours2.size();i++ )
00617 {
00618
00619
00620
00621 if ( minSize< contours2[i].size() &&contours2[i].size()<maxSize )
00622 {
00623
00624 approxPolyDP ( contours2[i] ,approxCurve , double ( contours2[i].size() ) *0.05 , true );
00625
00626
00627 if ( approxCurve.size() ==4 )
00628 {
00629
00630
00631
00632
00633
00634
00635 if ( isContourConvex ( Mat ( approxCurve ) ) )
00636 {
00637
00638
00639 float minDist=1e10;
00640 for ( int j=0;j<4;j++ )
00641 {
00642 float d= std::sqrt ( ( float ) ( approxCurve[j].x-approxCurve[ ( j+1 ) %4].x ) * ( approxCurve[j].x-approxCurve[ ( j+1 ) %4].x ) +
00643 ( approxCurve[j].y-approxCurve[ ( j+1 ) %4].y ) * ( approxCurve[j].y-approxCurve[ ( j+1 ) %4].y ) );
00644
00645 if ( d<minDist ) minDist=d;
00646 }
00647
00648 if ( minDist>10 )
00649 {
00650
00651
00652 MarkerCanditates.push_back ( MarkerCandidate() );
00653 MarkerCanditates.back().idx=i;
00654 for ( int j=0;j<4;j++ )
00655 {
00656 MarkerCanditates.back().push_back ( Point2f ( approxCurve[j].x,approxCurve[j].y ) );
00657 }
00658 }
00659 }
00660 }
00661 }
00662 }
00663
00664
00665
00666
00668 valarray<bool> swapped(false,MarkerCanditates.size());
00669 for ( unsigned int i=0;i<MarkerCanditates.size();i++ )
00670 {
00671
00672
00673
00674 double dx1 = MarkerCanditates[i][1].x - MarkerCanditates[i][0].x;
00675 double dy1 = MarkerCanditates[i][1].y - MarkerCanditates[i][0].y;
00676 double dx2 = MarkerCanditates[i][2].x - MarkerCanditates[i][0].x;
00677 double dy2 = MarkerCanditates[i][2].y - MarkerCanditates[i][0].y;
00678 double o = ( dx1*dy2 )- ( dy1*dx2 );
00679
00680 if ( o < 0.0 )
00681 {
00682 swap ( MarkerCanditates[i][1],MarkerCanditates[i][3] );
00683 swapped[i]=true;
00684
00685
00686
00687 }
00688 }
00689
00691
00692
00693 vector<pair<int,int> > TooNearCandidates;
00694 for ( unsigned int i=0;i<MarkerCanditates.size();i++ )
00695 {
00696
00697
00698 for ( unsigned int j=i+1;j<MarkerCanditates.size();j++ )
00699 {
00700 float dist=0;
00701 for ( int c=0;c<4;c++ )
00702 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 ) );
00703 dist/=4;
00704
00705 if ( dist< 10 )
00706 {
00707 TooNearCandidates.push_back ( pair<int,int> ( i,j ) );
00708 }
00709 }
00710 }
00711
00712
00713 valarray<bool> toRemove ( false,MarkerCanditates.size() );
00714 for ( unsigned int i=0;i<TooNearCandidates.size();i++ )
00715 {
00716 if ( perimeter ( MarkerCanditates[TooNearCandidates[i].first ] ) >perimeter ( MarkerCanditates[ TooNearCandidates[i].second] ) )
00717 toRemove[TooNearCandidates[i].second]=true;
00718 else toRemove[TooNearCandidates[i].first]=true;
00719 }
00720
00721
00722
00723
00724 OutMarkerCanditates.reserve(MarkerCanditates.size());
00725 for (size_t i=0;i<MarkerCanditates.size();i++) {
00726 if (!toRemove[i]) {
00727 OutMarkerCanditates.push_back(MarkerCanditates[i]);
00728 OutMarkerCanditates.back().contour=contours2[ MarkerCanditates[i].idx];
00729 if (swapped[i] && _enableCylinderWarp )
00730 reverse(OutMarkerCanditates.back().contour.begin(),OutMarkerCanditates.back().contour.end());
00731 }
00732 }
00733
00734 }
00735
00736
00737
00738
00739
00740
00741
00742 void MarkerDetector::thresHold ( int method,const Mat &grey,Mat &out,double param1,double param2 ) throw ( cv::Exception )
00743 {
00744
00745 if (param1==-1) param1=_thresParam1;
00746 if (param2==-1) param2=_thresParam2;
00747
00748 if ( grey.type() !=CV_8UC1 ) throw cv::Exception ( 9001,"grey.type()!=CV_8UC1","MarkerDetector::thresHold",__FILE__,__LINE__ );
00749 switch ( method )
00750 {
00751 case FIXED_THRES:
00752 cv::threshold ( grey, out, param1,255, CV_THRESH_BINARY_INV );
00753 break;
00754 case ADPT_THRES:
00755
00756 if ( param1<3 ) param1=3;
00757 else if ( ( ( int ) param1 ) %2 !=1 ) param1= ( int ) ( param1+1 );
00758
00759 cv::adaptiveThreshold ( grey,out,255,ADAPTIVE_THRESH_MEAN_C,THRESH_BINARY_INV,param1,param2 );
00760 break;
00761 case CANNY:
00762 {
00763
00764
00765
00766
00767 cv::Canny ( grey, out, 10, 220 );
00768
00769
00770
00771
00772
00773 }
00774 break;
00775 }
00776 }
00777
00778
00779
00780
00781
00782
00783 bool MarkerDetector::warp ( Mat &in,Mat &out,Size size, vector<Point2f> points ) throw ( cv::Exception )
00784 {
00785
00786 if ( points.size() !=4 ) throw cv::Exception ( 9001,"point.size()!=4","MarkerDetector::warp",__FILE__,__LINE__ );
00787
00788 Point2f pointsRes[4],pointsIn[4];
00789 for ( int i=0;i<4;i++ ) pointsIn[i]=points[i];
00790 pointsRes[0]= ( Point2f ( 0,0 ) );
00791 pointsRes[1]= Point2f ( size.width-1,0 );
00792 pointsRes[2]= Point2f ( size.width-1,size.height-1 );
00793 pointsRes[3]= Point2f ( 0,size.height-1 );
00794 Mat M=getPerspectiveTransform ( pointsIn,pointsRes );
00795 cv::warpPerspective ( in, out, M, size,cv::INTER_NEAREST );
00796 return true;
00797 }
00798
00799 void findCornerPointsInContour(const vector<cv::Point2f>& points,const vector<cv::Point> &contour,vector<int> &idxs)
00800 {
00801 assert(points.size()==4);
00802 int idxSegments[4]={-1,-1,-1,-1};
00803
00804 cv::Point points2i[4];
00805 for (int i=0;i<4;i++) {
00806 points2i[i].x=points[i].x;
00807 points2i[i].y=points[i].y;
00808 }
00809
00810 for (size_t i=0;i<contour.size();i++) {
00811 if (idxSegments[0]==-1)
00812 if (contour[i]==points2i[0]) idxSegments[0]=i;
00813 if (idxSegments[1]==-1)
00814 if (contour[i]==points2i[1]) idxSegments[1]=i;
00815 if (idxSegments[2]==-1)
00816 if (contour[i]==points2i[2]) idxSegments[2]=i;
00817 if (idxSegments[3]==-1)
00818 if (contour[i]==points2i[3]) idxSegments[3]=i;
00819 }
00820 idxs.resize(4);
00821 for (int i=0;i<4;i++) idxs[i]=idxSegments[i];
00822 }
00823
00824 int findDeformedSidesIdx(const vector<cv::Point> &contour,const vector<int> &idxSegments)
00825 {
00826 float distSum[4]={0,0,0,0};
00827 cv::Scalar colors[4]={cv::Scalar(0,0,255),cv::Scalar(255,0,0),cv::Scalar(0,255,0),cv::Scalar(111,111,0)};
00828
00829 for (int i=0;i<3;i++) {
00830 cv::Point p1=contour[ idxSegments[i]];
00831 cv::Point p2=contour[ idxSegments[i+1]];
00832 float inv_den=1./ sqrt(float(( p2.x-p1.x)*(p2.x-p1.x)+ (p2.y-p1.y)*(p2.y-p1.y)));
00833
00834
00835 for (size_t j=idxSegments[i];j<idxSegments[i+1];j++) {
00836 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;
00837 distSum[i]+=dist;
00838
00839
00840 }
00841 distSum[i]/=float(idxSegments[i+1]-idxSegments[i]);
00842
00843 }
00844
00845
00846
00847 cv::Point p1=contour[ idxSegments[0]];
00848 cv::Point p2=contour[ idxSegments[3]];
00849 float inv_den=1./ std::sqrt(float(( p2.x-p1.x)*(p2.x-p1.x)+ (p2.y-p1.y)*(p2.y-p1.y)));
00850
00851 for (size_t j=0;j<idxSegments[0];j++)
00852 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;
00853 for (size_t j=idxSegments[3];j<contour.size();j++)
00854 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;
00855
00856 distSum[3]/=float( idxSegments[0]+ (contour.size()-idxSegments[3]));
00857
00858
00859
00860
00861 if ( distSum[0]+distSum[2]> distSum[1]+distSum[3])
00862 return 0;
00863 else return 1;
00864 }
00865
00866 void setPointIntoImage(cv::Point2f &p,cv::Size s) {
00867 if (p.x<0) p.x=0;
00868 else if (p.x>=s.width )p.x=s.width-1;
00869 if (p.y<0)p.y=0;
00870 else if (p.y>=s.height)p.y=s.height-1;
00871
00872 }
00873
00874 void setPointIntoImage(cv::Point &p,cv::Size s) {
00875 if (p.x<0) p.x=0;
00876 else if (p.x>=s.width )p.x=s.width-1;
00877 if (p.y<0)p.y=0;
00878 else if (p.y>=s.height)p.y=s.height-1;
00879
00880 }
00881
00882
00883
00884
00885
00886
00887 bool MarkerDetector::warp_cylinder ( Mat &in,Mat &out,Size size, MarkerCandidate& mcand ) throw ( cv::Exception )
00888 {
00889
00890 if ( mcand.size() !=4 ) throw cv::Exception ( 9001,"point.size()!=4","MarkerDetector::warp",__FILE__,__LINE__ );
00891
00892
00893
00894
00895
00896
00897
00898
00899
00900 vector<int> idxSegments;
00901 findCornerPointsInContour(mcand,mcand.contour,idxSegments);
00902
00903 int minIdx=0;
00904 for (int i=1;i<4;i++)
00905 if (idxSegments[i] <idxSegments[minIdx]) minIdx=i;
00906
00907 std::rotate(idxSegments.begin(),idxSegments.begin()+minIdx,idxSegments.end());
00908 std::rotate(mcand.begin(),mcand.begin()+minIdx,mcand.end());
00909
00910
00911
00912 int defrmdSide=findDeformedSidesIdx(mcand.contour,idxSegments);
00913
00914
00915
00916
00917 cv::Point2f center=mcand.getCenter();
00918 Point2f enlargedRegion[4];
00919 for (int i=0;i<4;i++) enlargedRegion[i]=mcand[i];
00920 if (defrmdSide==0) {
00921 enlargedRegion[0]=mcand[0]+(mcand[3]-mcand[0])*1.2;
00922 enlargedRegion[1]=mcand[1]+(mcand[2]-mcand[1])*1.2;
00923 enlargedRegion[2]=mcand[2]+(mcand[1]-mcand[2])*1.2;
00924 enlargedRegion[3]=mcand[3]+(mcand[0]-mcand[3])*1.2;
00925 }
00926 else {
00927 enlargedRegion[0]=mcand[0]+(mcand[1]-mcand[0])*1.2;
00928 enlargedRegion[1]=mcand[1]+(mcand[0]-mcand[1])*1.2;
00929 enlargedRegion[2]=mcand[2]+(mcand[3]-mcand[2])*1.2;
00930 enlargedRegion[3]=mcand[3]+(mcand[2]-mcand[3])*1.2;
00931 }
00932 for (size_t i=0;i<4;i++)
00933 setPointIntoImage(enlargedRegion[i],in.size());
00934
00935
00936
00937
00938
00939
00940
00941
00942
00943
00944
00945
00946
00947
00948
00949
00950 Point2f pointsRes[4],pointsIn[4];
00951 for ( int i=0;i<4;i++ ) pointsIn[i]=mcand[i];
00952
00953 cv::Size enlargedSize=size;
00954 enlargedSize.width+=2*enlargedSize.width*0.2;
00955 pointsRes[0]= ( Point2f ( 0,0 ) );
00956 pointsRes[1]= Point2f ( enlargedSize.width-1,0 );
00957 pointsRes[2]= Point2f ( enlargedSize.width-1,enlargedSize.height-1 );
00958 pointsRes[3]= Point2f ( 0,enlargedSize.height-1 );
00959
00960 if (defrmdSide==0) rotate(pointsRes,pointsRes+1,pointsRes+4);
00961 cv::Mat imAux,imAux2(enlargedSize,CV_8UC1);
00962 Mat M=getPerspectiveTransform ( enlargedRegion,pointsRes );
00963 cv::warpPerspective ( in, imAux, M, enlargedSize,cv::INTER_NEAREST);
00964
00965
00966 vector<cv::Point> pointsCO(mcand.contour.size());
00967 assert(M.type()==CV_64F);
00968 assert(M.cols==3 && M.rows==3);
00969
00970 double *mptr=M.ptr<double>(0);
00971 imAux2.setTo(cv::Scalar::all(0));
00972
00973
00974 for (size_t i=0;i<mcand.contour.size();i++) {
00975 float inX=mcand.contour[i].x;
00976 float inY=mcand.contour[i].y;
00977 float w= inX * mptr[6]+inY * mptr[7]+mptr[8];
00978 cv::Point2f pres;
00979 pointsCO[i].x=( (inX * mptr[0]+inY* mptr[1]+mptr[2])/w)+0.5;
00980 pointsCO[i].y=( (inX * mptr[3]+inY* mptr[4]+mptr[5])/w)+0.5;
00981
00982 setPointIntoImage(pointsCO[i],imAux.size());
00983
00984 imAux2.at<uchar>(pointsCO[i].y,pointsCO[i].x)=255;
00985 if (pointsCO[i].y>0)
00986 imAux2.at<uchar>(pointsCO[i].y-1,pointsCO[i].x)=255;
00987 if (pointsCO[i].y<imAux2.rows-1 )
00988 imAux2.at<uchar>(pointsCO[i].y+1,pointsCO[i].x)=255;
00989 }
00990
00991 cv::Mat outIm(enlargedSize,CV_8UC1);
00992 outIm.setTo(cv::Scalar::all(0));
00993
00994 for (int y=0;y<imAux2.rows;y++) {
00995 uchar *_offInfo=imAux2.ptr<uchar>(y);
00996 int start=-1,end=-1;
00997
00998 for (int x=0;x<imAux.cols;x++) {
00999 if (_offInfo[x]) {
01000 if (start==-1) start=x;
01001 else end=x;
01002 }
01003 }
01004
01005
01006 assert(start!=-1 && end!=-1 && (end-start)> size.width>>1);
01007 uchar *In_image=imAux.ptr<uchar>(y);
01008 uchar *Out_image=outIm.ptr<uchar>(y);
01009 memcpy(Out_image,In_image+start,imAux.cols-start );
01010 }
01011
01012
01013
01014
01015 cv::Mat centerReg=outIm(cv::Range::all(),cv::Range(0,size.width));
01016 out=centerReg.clone();
01017
01018
01019
01020
01021
01022 return true;
01023 }
01024
01025
01026
01027
01028
01029
01030 bool MarkerDetector::isInto ( Mat &contour,vector<Point2f> &b )
01031 {
01032
01033 for ( unsigned int i=0;i<b.size();i++ )
01034 if ( pointPolygonTest ( contour,b[i],false ) >0 ) return true;
01035 return false;
01036 }
01037
01038
01039
01040
01041
01042
01043 int MarkerDetector:: perimeter ( vector<Point2f> &a )
01044 {
01045 int sum=0;
01046 for ( unsigned int i=0;i<a.size();i++ )
01047 {
01048 int i2= ( i+1 ) %a.size();
01049 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 ) ) ;
01050 }
01051 return sum;
01052 }
01053
01054
01059 void MarkerDetector::findBestCornerInRegion_harris ( const cv::Mat & grey,vector<cv::Point2f> & Corners,int blockSize )
01060 {
01061 int halfSize=blockSize/2;
01062 for ( size_t i=0;i<Corners.size();i++ )
01063 {
01064
01065 cv::Point2f min ( Corners[i].x-halfSize,Corners[i].y-halfSize );
01066 cv::Point2f max ( Corners[i].x+halfSize,Corners[i].y+halfSize );
01067 if ( min.x>=0 && min.y>=0 && max.x<grey.cols && max.y<grey.rows )
01068 {
01069 cv::Mat response;
01070 cv::Mat subImage ( grey,cv::Rect ( Corners[i].x-halfSize,Corners[i].y-halfSize,blockSize ,blockSize ) );
01071 vector<Point2f> corners2;
01072 goodFeaturesToTrack ( subImage, corners2, 10, 0.001, halfSize );
01073 float minD=9999;
01074 int bIdx=-1;
01075 cv::Point2f Center ( halfSize,halfSize );
01076 for ( size_t j=0;j<corners2.size();j++ )
01077 {
01078 float dist=cv::norm ( corners2[j]-Center );
01079 if ( dist<minD )
01080 {
01081 minD=dist;
01082 bIdx=j;
01083 }
01084 if ( minD<halfSize ) Corners[i]+= ( corners2[bIdx]-Center );
01085 }
01086 }
01087 }
01088 }
01089
01090
01095 void MarkerDetector::refineCandidateLines(MarkerDetector::MarkerCandidate& candidate)
01096 {
01097
01098 vector<unsigned int> cornerIndex;
01099 cornerIndex.resize(4);
01100 for(unsigned int j=0; j<candidate.contour.size(); j++) {
01101 for(unsigned int k=0; k<4; k++) {
01102 if(candidate.contour[j].x==candidate[k].x && candidate.contour[j].y==candidate[k].y) {
01103 cornerIndex[k] = j;
01104 }
01105 }
01106 }
01107
01108
01109 bool inverse;
01110 if( (cornerIndex[1] > cornerIndex[0]) && (cornerIndex[2]>cornerIndex[1] || cornerIndex[2]<cornerIndex[0]) )
01111 inverse = false;
01112 else if( cornerIndex[2]>cornerIndex[1] && cornerIndex[2]<cornerIndex[0] )
01113 inverse = false;
01114 else inverse = true;
01115
01116
01117
01118 int inc = 1;
01119 if(inverse) inc = -1;
01120
01121 vector<std::vector<cv::Point> > contourLines;
01122 contourLines.resize(4);
01123 for(unsigned int l=0; l<4; l++) {
01124 for(int j=(int)cornerIndex[l]; j!=(int)cornerIndex[(l+1)%4]; j+=inc) {
01125 if(j==(int)candidate.contour.size() && !inverse) j=0;
01126 else if(j==0 && inverse) j=candidate.contour.size()-1;
01127 contourLines[l].push_back(candidate.contour[j]);
01128 if(j==(int)cornerIndex[(l+1)%4]) break;
01129 }
01130
01131 }
01132
01133
01134 vector<Point3f> lines;
01135 lines.resize(4);
01136 for(unsigned int j=0; j<lines.size(); j++) interpolate2Dline(contourLines[j], lines[j]);
01137
01138
01139 vector<Point2f> crossPoints;
01140 crossPoints.resize(4);
01141 for(unsigned int i=0; i<4; i++)
01142 crossPoints[i] = getCrossPoint( lines[(i-1)%4], lines[i] );
01143
01144
01145 for(unsigned int j=0; j<4; j++)
01146 candidate[j] = crossPoints[j];
01147 }
01148
01149
01152 void MarkerDetector::interpolate2Dline( const std::vector< Point >& inPoints, Point3f& outLine)
01153 {
01154
01155 float minX, maxX, minY, maxY;
01156 minX = maxX = inPoints[0].x;
01157 minY = maxY = inPoints[0].y;
01158 for(unsigned int i=1; i<inPoints.size(); i++) {
01159 if(inPoints[i].x < minX) minX = inPoints[i].x;
01160 if(inPoints[i].x > maxX) maxX = inPoints[i].x;
01161 if(inPoints[i].y < minY) minY = inPoints[i].y;
01162 if(inPoints[i].y > maxY) maxY = inPoints[i].y;
01163 }
01164
01165
01166 Mat A(inPoints.size(),2,CV_32FC1, Scalar(0));
01167 Mat B(inPoints.size(),1,CV_32FC1, Scalar(0));
01168 Mat X;
01169
01170
01171
01172 if( maxX-minX > maxY-minY ) {
01173
01174 for (int i=0; i<inPoints.size(); i++) {
01175
01176 A.at<float>(i, 0) = inPoints[i].x;
01177 A.at<float>(i, 1) = 1.;
01178 B.at<float>(i, 0) = inPoints[i].y;
01179
01180 }
01181
01182
01183 solve(A,B,X, DECOMP_SVD);
01184
01185 outLine = Point3f(X.at<float>(0,0), -1., X.at<float>(1,0));
01186 }
01187 else {
01188
01189 for (int i=0; i<inPoints.size(); i++) {
01190
01191 A.at<float>(i, 0) = inPoints[i].y;
01192 A.at<float>(i, 1) = 1.;
01193 B.at<float>(i, 0) = inPoints[i].x;
01194
01195 }
01196
01197
01198 solve(A,B,X, DECOMP_SVD);
01199
01200 outLine = Point3f(-1., X.at<float>(0,0), X.at<float>(1,0));
01201 }
01202
01203 }
01204
01207 Point2f MarkerDetector::getCrossPoint(const cv::Point3f& line1, const cv::Point3f& line2)
01208 {
01209
01210
01211 Mat A(2,2,CV_32FC1, Scalar(0));
01212 Mat B(2,1,CV_32FC1, Scalar(0));
01213 Mat X;
01214
01215 A.at<float>(0, 0) = line1.x;
01216 A.at<float>(0, 1) = line1.y;
01217 B.at<float>(0, 0) = -line1.z;
01218
01219 A.at<float>(1, 0) = line2.x;
01220 A.at<float>(1, 1) = line2.y;
01221 B.at<float>(1, 0) = -line2.z;
01222
01223
01224 solve(A,B,X, DECOMP_SVD);
01225 return Point2f(X.at<float>(0,0), X.at<float>(1,0));
01226
01227 }
01228
01229
01230
01231
01232
01233
01234
01235
01236
01237
01238
01239
01240
01241
01242 void MarkerDetector::drawAllContours ( Mat input, std::vector<std::vector<cv::Point> > &contours )
01243 {
01244 drawContours ( input, contours, -1,Scalar ( 255,0,255 ) );
01245 }
01246
01247
01248
01249
01250
01251
01252
01253 void MarkerDetector:: drawContour ( Mat &in,vector<Point> &contour,Scalar color )
01254 {
01255 for ( unsigned int i=0;i<contour.size();i++ )
01256 {
01257 cv::rectangle ( in,contour[i],contour[i],color );
01258 }
01259 }
01260
01261 void MarkerDetector:: drawApproxCurve ( Mat &in,vector<Point> &contour,Scalar color )
01262 {
01263 for ( unsigned int i=0;i<contour.size();i++ )
01264 {
01265 cv::line ( in,contour[i],contour[ ( i+1 ) %contour.size() ],color );
01266 }
01267 }
01268
01269
01270
01271
01272
01273
01274
01275 void MarkerDetector::draw ( Mat out,const vector<Marker> &markers )
01276 {
01277 for ( unsigned int i=0;i<markers.size();i++ )
01278 {
01279 cv::line ( out,markers[i][0],markers[i][1],cvScalar ( 255,0,0 ),2,CV_AA );
01280 cv::line ( out,markers[i][1],markers[i][2],cvScalar ( 255,0,0 ),2,CV_AA );
01281 cv::line ( out,markers[i][2],markers[i][3],cvScalar ( 255,0,0 ),2,CV_AA );
01282 cv::line ( out,markers[i][3],markers[i][0],cvScalar ( 255,0,0 ),2,CV_AA );
01283 }
01284 }
01285
01286
01287
01288
01289
01290
01291
01292
01293
01294
01295
01296
01297
01298
01299
01300
01301
01302
01303
01304
01305
01306
01307
01308
01309
01310
01311
01312
01313
01314
01315
01316
01317 void MarkerDetector::glGetProjectionMatrix ( CameraParameters & CamMatrix,cv::Size orgImgSize, cv::Size size,double proj_matrix[16],double gnear,double gfar,bool invert ) throw ( cv::Exception )
01318 {
01319 cerr<<"MarkerDetector::glGetProjectionMatrix . This a deprecated function. Use CameraParameters::glGetProjectionMatrix instead. "<<__FILE__<<" "<<__LINE__<<endl;
01320 CamMatrix.glGetProjectionMatrix ( orgImgSize,size,proj_matrix,gnear,gfar,invert );
01321 }
01322
01323
01324
01325
01326
01327
01328
01329
01330 void MarkerDetector::setMinMaxSize(float min ,float max )throw(cv::Exception)
01331 {
01332 if (min<=0 || min>1) throw cv::Exception(1," min parameter out of range","MarkerDetector::setMinMaxSize",__FILE__,__LINE__);
01333 if (max<=0 || max>1) throw cv::Exception(1," max parameter out of range","MarkerDetector::setMinMaxSize",__FILE__,__LINE__);
01334 if (min>max) throw cv::Exception(1," min>max","MarkerDetector::setMinMaxSize",__FILE__,__LINE__);
01335 _minSize=min;
01336 _maxSize=max;
01337 }
01338
01339 };
01340