28 #include <aruco/markerdetector.h> 29 #include <opencv2/opencv.hpp> 30 #include <opencv2/highgui/highgui.hpp> 31 #include <opencv2/imgproc/imgproc.hpp> 34 #include <aruco/arucofidmarkers.h> 47 MarkerDetector::MarkerDetector()
50 _enableCylinderWarp=
false;
51 _thresMethod=ADPT_THRES;
52 _thresParam1=_thresParam2=7;
68 MarkerDetector::~MarkerDetector()
79 void MarkerDetector::setDesiredSpeed (
int val )
82 else if ( val>3 ) val=2;
108 void MarkerDetector::detect (
const cv::Mat &input,std::vector<Marker> &detectedMarkers,
CameraParameters camParams ,
float markerSizeMeters ,
bool setYPerpendicular)
throw ( cv::Exception )
110 detect ( input, detectedMarkers,camParams.CameraMatrix ,camParams.Distorsion, markerSizeMeters ,setYPerpendicular);
120 void MarkerDetector::detect (
const cv::Mat &input,vector<Marker> &detectedMarkers,Mat camMatrix ,Mat distCoeff ,
float markerSizeMeters ,
bool setYPerpendicular)
throw ( cv::Exception )
125 if ( input.type() ==CV_8UC3 ) cv::cvtColor ( input,grey,cv::COLOR_BGR2GRAY );
132 detectedMarkers.clear();
135 cv::Mat imgToBeThresHolded=grey;
136 double ThresParam1=_thresParam1,ThresParam2=_thresParam2;
138 if ( pyrdown_level!=0 )
141 for (
int i=0;i<pyrdown_level;i++ )
144 cv::pyrDown ( reduced,tmp );
147 int red_den=pow ( 2.0
f,pyrdown_level );
148 imgToBeThresHolded=reduced;
149 ThresParam1/=float ( red_den );
150 ThresParam2/=float ( red_den );
154 thresHold ( _thresMethod,imgToBeThresHolded,thres,ThresParam1,ThresParam2 );
159 erode ( thres,thres2,cv::Mat() );
164 vector<MarkerCandidate > MarkerCanditates;
165 detectRectangles ( thres,MarkerCanditates );
167 if ( pyrdown_level!=0 )
169 float red_den=pow ( 2.0
f,pyrdown_level );
170 float offInc= ( ( pyrdown_level/2. )-0.5 );
171 for (
unsigned int i=0;i<MarkerCanditates.size();++i ) {
172 for (
int c=0;c<4;c++ )
174 MarkerCanditates[i][c].x=MarkerCanditates[i][c].x*red_den+offInc;
175 MarkerCanditates[i][c].y=MarkerCanditates[i][c].y*red_den+offInc;
178 for (
size_t c=0;c<MarkerCanditates[i].contour.size();++c )
180 MarkerCanditates[i].contour[c].x=MarkerCanditates[i].contour[c].x*red_den+offInc;
181 MarkerCanditates[i].contour[c].y=MarkerCanditates[i].contour[c].y*red_den+offInc;
188 for (
unsigned int i=0;i<MarkerCanditates.size();++i )
193 if (_enableCylinderWarp)
194 resW=warp_cylinder( grey,canonicalMarker,Size ( _markerWarpSize,_markerWarpSize ),MarkerCanditates[i] );
195 else resW=warp ( grey,canonicalMarker,Size ( _markerWarpSize,_markerWarpSize ),MarkerCanditates[i] );
198 int id= ( *markerIdDetector_ptrfunc ) ( canonicalMarker,nRotations );
201 if(_cornerMethod==LINES) refineCandidateLines( MarkerCanditates[i] );
202 detectedMarkers.push_back ( MarkerCanditates[i] );
203 detectedMarkers.back().id=id;
205 std::rotate ( detectedMarkers.back().begin(),detectedMarkers.back().begin() +4-nRotations,detectedMarkers.back().end() );
207 else _candidates.push_back ( MarkerCanditates[i] );
214 if ( detectedMarkers.size() >0 && _cornerMethod!=
NONE && _cornerMethod!=LINES )
216 vector<Point2f> Corners;
217 for (
unsigned int i=0;i<detectedMarkers.size();++i )
218 for (
int c=0;c<4;c++ )
219 Corners.push_back ( detectedMarkers[i][c] );
221 if ( _cornerMethod==HARRIS )
222 findBestCornerInRegion_harris ( grey, Corners,7 );
223 else if ( _cornerMethod==SUBPIX )
224 cornerSubPix ( grey, Corners,cv::Size ( 5,5 ), cv::Size ( -1,-1 ) ,cv::TermCriteria ( cv::TermCriteria::EPS | cv::TermCriteria::COUNT,3,0.05 ) );
227 for (
unsigned int i=0;i<detectedMarkers.size();++i )
228 for (
int c=0;c<4;c++ ) detectedMarkers[i][c]=Corners[i*4+c];
231 std::sort ( detectedMarkers.begin(),detectedMarkers.end() );
234 vector<bool> toRemove ( detectedMarkers.size(),false );
235 for (
int i=0;i<int ( detectedMarkers.size() )-1;++i )
237 if ( detectedMarkers[i].
id==detectedMarkers[i+1].
id && !toRemove[i+1] )
240 if ( perimeter ( detectedMarkers[i] ) >perimeter ( detectedMarkers[i+1] ) ) toRemove[i+1]=
true;
241 else toRemove[i]=
true;
245 removeElements ( detectedMarkers, toRemove );
248 if ( camMatrix.rows!=0 && markerSizeMeters>0 )
250 for (
unsigned int i=0;i<detectedMarkers.size();i++ )
251 detectedMarkers[i].calculateExtrinsics ( markerSizeMeters,camMatrix,distCoeff,setYPerpendicular );
262 void MarkerDetector::detectRectangles (
const cv::Mat &thresold,vector<std::vector<cv::Point2f> > &MarkerCanditates )
264 vector<MarkerCandidate> candidates;
265 detectRectangles(thresold,candidates);
267 MarkerCanditates.resize(candidates.size());
268 for (
size_t i=0;i<MarkerCanditates.size();i++)
269 MarkerCanditates[i]=candidates[i];
272 void MarkerDetector::detectRectangles(
const cv::Mat &thresImg,vector<MarkerCandidate> & OutMarkerCanditates)
274 vector<MarkerCandidate> MarkerCanditates;
276 unsigned int minSize=_minSize*std::max(thresImg.cols,thresImg.rows)*4;
277 unsigned int maxSize=_maxSize*std::max(thresImg.cols,thresImg.rows)*4;
278 std::vector<std::vector<cv::Point> > contours2;
279 std::vector<cv::Vec4i> hierarchy2;
281 thresImg.copyTo ( thres2 );
282 cv::findContours ( thres2 , contours2, hierarchy2, cv::RETR_TREE, cv::CHAIN_APPROX_NONE );
283 vector<Point> approxCurve;
286 for (
unsigned int i=0;i<contours2.size();i++ )
291 if ( minSize< contours2[i].size() &&contours2[i].size()<maxSize )
294 approxPolyDP ( contours2[i] ,approxCurve ,
double ( contours2[i].size() ) *0.05 ,
true );
297 if ( approxCurve.size() ==4 )
305 if ( isContourConvex ( Mat ( approxCurve ) ) )
310 for (
int j=0;j<4;j++ )
312 float d= std::sqrt ( (
float ) ( approxCurve[j].
x-approxCurve[ ( j+1 ) %4].
x ) * ( approxCurve[j].
x-approxCurve[ ( j+1 ) %4].
x ) +
313 ( approxCurve[j].
y-approxCurve[ ( j+1 ) %4].
y ) * ( approxCurve[j].
y-approxCurve[ ( j+1 ) %4].
y ) );
315 if ( d<minDist ) minDist=d;
323 MarkerCanditates.back().idx=i;
324 for (
int j=0;j<4;j++ )
326 MarkerCanditates.back().push_back ( Point2f ( approxCurve[j].
x,approxCurve[j].
y ) );
338 valarray<bool> swapped(
false,MarkerCanditates.size());
339 for (
unsigned int i=0;i<MarkerCanditates.size();i++ )
344 double dx1 = MarkerCanditates[i][1].x - MarkerCanditates[i][0].x;
345 double dy1 = MarkerCanditates[i][1].y - MarkerCanditates[i][0].y;
346 double dx2 = MarkerCanditates[i][2].x - MarkerCanditates[i][0].x;
347 double dy2 = MarkerCanditates[i][2].y - MarkerCanditates[i][0].y;
348 double o = ( dx1*dy2 )- ( dy1*dx2 );
352 swap ( MarkerCanditates[i][1],MarkerCanditates[i][3] );
363 vector<pair<int,int> > TooNearCandidates;
364 for (
unsigned int i=0;i<MarkerCanditates.size();i++ )
368 for (
unsigned int j=i+1;j<MarkerCanditates.size();j++ )
371 for (
int c=0;c<4;c++ )
372 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 ) );
377 TooNearCandidates.push_back ( pair<int,int> ( i,j ) );
383 valarray<bool> toRemove (
false,MarkerCanditates.size() );
384 for (
unsigned int i=0;i<TooNearCandidates.size();i++ )
386 if ( perimeter ( MarkerCanditates[TooNearCandidates[i].first ] ) >perimeter ( MarkerCanditates[ TooNearCandidates[i].second] ) )
387 toRemove[TooNearCandidates[i].second]=
true;
388 else toRemove[TooNearCandidates[i].first]=
true;
394 OutMarkerCanditates.reserve(MarkerCanditates.size());
395 for (
size_t i=0;i<MarkerCanditates.size();i++) {
397 OutMarkerCanditates.push_back(MarkerCanditates[i]);
398 OutMarkerCanditates.back().contour=contours2[ MarkerCanditates[i].idx];
399 if (swapped[i] && _enableCylinderWarp )
400 reverse(OutMarkerCanditates.back().contour.begin(),OutMarkerCanditates.back().contour.end());
412 void MarkerDetector::thresHold (
int method,
const Mat &grey_m,Mat &out,
double param1,
double param2 )
throw ( cv::Exception )
414 #pragma GCC diagnostic push 415 #pragma GCC diagnostic ignored "-Wfloat-equal" 416 if (param1==-1) param1=_thresParam1;
417 if (param2==-1) param2=_thresParam2;
418 #pragma GCC diagnostic pop 420 if ( grey_m.type() !=CV_8UC1 )
throw cv::Exception ( 9001,
"grey_m.type()!=CV_8UC1",
"MarkerDetector::thresHold",__FILE__,__LINE__ );
424 cv::threshold ( grey_m, out, param1,255, cv::THRESH_BINARY_INV );
428 if ( param1<3 ) param1=3;
429 else if ( ( (
int ) param1 ) %2 !=1 ) param1= ( int ) ( param1+1 );
431 cv::adaptiveThreshold ( grey_m,out,255,ADAPTIVE_THRESH_MEAN_C,cv::THRESH_BINARY_INV,param1,param2 );
439 cv::Canny ( grey_m, out, 10, 220 );
455 bool MarkerDetector::warp ( Mat &in,Mat &out,Size size, vector<Point2f> points )
throw ( cv::Exception )
458 if ( points.size() !=4 )
throw cv::Exception ( 9001,
"point.size()!=4",
"MarkerDetector::warp",__FILE__,__LINE__ );
460 Point2f pointsRes[4],pointsIn[4];
461 for (
int i=0;i<4;i++ ) pointsIn[i]=points[i];
462 pointsRes[0]= ( Point2f ( 0,0 ) );
463 pointsRes[1]= Point2f ( size.width-1,0 );
464 pointsRes[2]= Point2f ( size.width-1,size.height-1 );
465 pointsRes[3]= Point2f ( 0,size.height-1 );
466 Mat M=getPerspectiveTransform ( pointsIn,pointsRes );
467 cv::warpPerspective ( in, out, M, size,cv::INTER_NEAREST );
473 assert(points.size()==4);
474 int idxSegments[4]={-1,-1,-1,-1};
476 cv::Point points2i[4];
477 for (
int i=0;i<4;i++) {
478 points2i[i].x=points[i].x;
479 points2i[i].y=points[i].y;
482 for (
size_t i=0;i<contour.size();++i) {
483 if (idxSegments[0]==-1)
484 if (contour[i]==points2i[0]) idxSegments[0]=i;
485 if (idxSegments[1]==-1)
486 if (contour[i]==points2i[1]) idxSegments[1]=i;
487 if (idxSegments[2]==-1)
488 if (contour[i]==points2i[2]) idxSegments[2]=i;
489 if (idxSegments[3]==-1)
490 if (contour[i]==points2i[3]) idxSegments[3]=i;
493 for (
int i=0;i<4;i++) idxs[i]=idxSegments[i];
498 float distSum[4]={0,0,0,0};
500 for (
int i=0;i<3;i++) {
501 cv::Point p1=contour[ idxSegments[i]];
502 cv::Point p2=contour[ idxSegments[i+1]];
503 float inv_den=1./ sqrt(
float(( p2.x-p1.x)*(p2.x-p1.x)+ (p2.y-p1.y)*(p2.y-p1.y)));
506 for (
int j=idxSegments[i];j<idxSegments[i+1];++j) {
507 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;
512 distSum[i]/=float(idxSegments[i+1]-idxSegments[i]);
518 cv::Point p1=contour[ idxSegments[0]];
519 cv::Point p2=contour[ idxSegments[3]];
520 float inv_den=1./ std::sqrt(
float(( p2.x-p1.x)*(p2.x-p1.x)+ (p2.y-p1.y)*(p2.y-p1.y)));
522 for (
int j=0;j<idxSegments[0];++j)
523 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;
524 for (
int j=idxSegments[3];j<static_cast<int>(contour.size());++j)
525 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;
527 distSum[3]/=float( idxSegments[0]+ (contour.size()-idxSegments[3]));
532 if ( distSum[0]+distSum[2]> distSum[1]+distSum[3])
539 else if (p.x>=s.width )p.x=s.width-1;
541 else if (p.y>=s.height)p.y=s.height-1;
547 else if (p.x>=s.width )p.x=s.width-1;
549 else if (p.y>=s.height)p.y=s.height-1;
558 bool MarkerDetector::warp_cylinder ( Mat &in,Mat &out,Size size, MarkerCandidate& mcand )
throw ( cv::Exception )
561 if ( mcand.size() !=4 )
throw cv::Exception ( 9001,
"point.size()!=4",
"MarkerDetector::warp",__FILE__,__LINE__ );
571 vector<int> idxSegments;
575 for (
int i=1;i<4;i++)
576 if (idxSegments[i] <idxSegments[minIdx]) minIdx=i;
578 std::rotate(idxSegments.begin(),idxSegments.begin()+minIdx,idxSegments.end());
579 std::rotate(mcand.begin(),mcand.begin()+minIdx,mcand.end());
588 Point2f enlargedRegion[4];
589 for (
int i=0;i<4;i++) enlargedRegion[i]=mcand[i];
591 enlargedRegion[0]=mcand[0]+(mcand[3]-mcand[0])*1.2;
592 enlargedRegion[1]=mcand[1]+(mcand[2]-mcand[1])*1.2;
593 enlargedRegion[2]=mcand[2]+(mcand[1]-mcand[2])*1.2;
594 enlargedRegion[3]=mcand[3]+(mcand[0]-mcand[3])*1.2;
597 enlargedRegion[0]=mcand[0]+(mcand[1]-mcand[0])*1.2;
598 enlargedRegion[1]=mcand[1]+(mcand[0]-mcand[1])*1.2;
599 enlargedRegion[2]=mcand[2]+(mcand[3]-mcand[2])*1.2;
600 enlargedRegion[3]=mcand[3]+(mcand[2]-mcand[3])*1.2;
602 for (
size_t i=0;i<4;i++)
620 Point2f pointsRes[4];
622 cv::Size enlargedSize=size;
623 enlargedSize.width+=2*enlargedSize.width*0.2;
624 pointsRes[0]= ( Point2f ( 0,0 ) );
625 pointsRes[1]= Point2f ( enlargedSize.width-1,0 );
626 pointsRes[2]= Point2f ( enlargedSize.width-1,enlargedSize.height-1 );
627 pointsRes[3]= Point2f ( 0,enlargedSize.height-1 );
629 if (defrmdSide==0)
rotate(pointsRes,pointsRes+1,pointsRes+4);
630 cv::Mat imAux,imAux2(enlargedSize,CV_8UC1);
631 Mat M=getPerspectiveTransform ( enlargedRegion,pointsRes );
632 cv::warpPerspective ( in, imAux, M, enlargedSize,cv::INTER_NEAREST);
635 vector<cv::Point> pointsCO(mcand.contour.size());
636 assert(M.type()==CV_64F);
637 assert(M.cols==3 && M.rows==3);
639 double *mptr=M.ptr<
double>(0);
640 imAux2.setTo(cv::Scalar::all(0));
643 for (
size_t i=0;i<mcand.contour.size();i++) {
644 float inX=mcand.contour[i].x;
645 float inY=mcand.contour[i].y;
646 float w= inX * mptr[6]+inY * mptr[7]+mptr[8];
648 pointsCO[i].x=( (inX * mptr[0]+inY* mptr[1]+mptr[2])/w)+0.5;
649 pointsCO[i].y=( (inX * mptr[3]+inY* mptr[4]+mptr[5])/w)+0.5;
653 imAux2.at<uchar>(pointsCO[i].y,pointsCO[i].x)=255;
655 imAux2.at<uchar>(pointsCO[i].y-1,pointsCO[i].x)=255;
656 if (pointsCO[i].
y<imAux2.rows-1 )
657 imAux2.at<uchar>(pointsCO[i].y+1,pointsCO[i].x)=255;
660 cv::Mat outIm(enlargedSize,CV_8UC1);
661 outIm.setTo(cv::Scalar::all(0));
663 for (
int y=0;
y<imAux2.rows;
y++) {
664 uchar *_offInfo=imAux2.ptr<uchar>(
y);
667 for (
int x=0;
x<imAux.cols;
x++) {
669 if (start==-1) start=x;
675 assert(start!=-1 && end!=-1 && (end-start)> size.width>>1);
676 uchar *In_image=imAux.ptr<uchar>(
y);
677 uchar *Out_image=outIm.ptr<uchar>(
y);
678 memcpy(Out_image,In_image+start,imAux.cols-start );
684 cv::Mat centerReg=outIm(cv::Range::all(),cv::Range(0,size.width));
685 out=centerReg.clone();
699 bool MarkerDetector::isInto ( Mat &contour,vector<Point2f> &b )
702 for (
size_t i=0;i<b.size();i++ )
703 if ( pointPolygonTest ( contour,b[i],
false ) >0 )
return true;
712 int MarkerDetector:: perimeter ( vector<Point2f> &a )
715 for (
size_t i=0;i<a.size();i++ )
717 int i2= ( i+1 ) %a.size();
718 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 ) ) ;
728 void MarkerDetector::findBestCornerInRegion_harris (
const cv::Mat & grey_m,vector<cv::Point2f> & Corners,
int blockSize )
730 int halfSize=blockSize/2;
731 for (
size_t i=0;i<Corners.size();i++ )
734 cv::Point2f
min ( Corners[i].
x-halfSize,Corners[i].
y-halfSize );
735 cv::Point2f max ( Corners[i].
x+halfSize,Corners[i].
y+halfSize );
736 if ( min.x>=0 && min.y>=0 && max.x<grey_m.cols && max.y<grey_m.rows )
739 cv::Mat subImage ( grey_m,cv::Rect ( Corners[i].
x-halfSize,Corners[i].
y-halfSize,blockSize ,blockSize ) );
740 vector<Point2f> corners2;
741 goodFeaturesToTrack ( subImage, corners2, 10, 0.001, halfSize );
744 cv::Point2f Center ( halfSize,halfSize );
745 for (
size_t j=0;j<corners2.size();j++ )
747 float dist=cv::norm ( corners2[j]-Center );
753 if ( minD<halfSize ) Corners[i]+= ( corners2[bIdx]-Center );
767 vector<size_t> cornerIndex;
768 cornerIndex.resize(4);
769 for(
size_t j=0; j<candidate.
contour.size(); ++j) {
770 for(
size_t k=0; k<4; ++k) {
771 #pragma GCC diagnostic push 772 #pragma GCC diagnostic ignored "-Wfloat-equal" 773 if(candidate.
contour[j].x==candidate[k].x && candidate.
contour[j].y==candidate[k].y) {
776 #pragma GCC diagnostic pop 782 if( (cornerIndex[1] > cornerIndex[0]) && (cornerIndex[2]>cornerIndex[1] || cornerIndex[2]<cornerIndex[0]) )
784 else if( cornerIndex[2]>cornerIndex[1] && cornerIndex[2]<cornerIndex[0] )
791 if(inverse) inc = -1;
793 vector<std::vector<cv::Point> > contourLines;
794 contourLines.resize(4);
795 for(
unsigned int l=0; l<4; ++l) {
796 for(
int j=(
int)cornerIndex[l]; j!=(int)cornerIndex[(l+1)%4]; j+=inc) {
797 if(j==(
int)candidate.
contour.size() && !inverse) j=0;
798 else if(j==0 && inverse) j=candidate.
contour.size()-1;
799 contourLines[l].push_back(candidate.
contour[j]);
800 if(j==(
int)cornerIndex[(l+1)%4])
break;
806 vector<Point3f> lines;
808 for(
unsigned int j=0; j<lines.size(); ++j)
809 interpolate2Dline(contourLines[j], lines[j]);
812 vector<Point2f> crossPoints;
813 crossPoints.resize(4);
814 for(
unsigned int i=0; i<4; ++i)
815 crossPoints[i] = getCrossPoint( lines[(i-1)%4], lines[i] );
818 for(
unsigned int j=0; j<4; ++j)
819 candidate[j] = crossPoints[j];
825 void MarkerDetector::interpolate2Dline(
const std::vector< Point >& inPoints, Point3f& outLine)
828 float minX, maxX, minY, maxY;
829 minX = maxX = inPoints[0].x;
830 minY = maxY = inPoints[0].y;
831 for(
unsigned int i=1; i<inPoints.size(); ++i) {
832 if(inPoints[i].
x < minX) minX = inPoints[i].x;
833 if(inPoints[i].
x > maxX) maxX = inPoints[i].x;
834 if(inPoints[i].
y < minY) minY = inPoints[i].y;
835 if(inPoints[i].
y > maxY) maxY = inPoints[i].y;
839 Mat A(inPoints.size(),2,CV_32FC1, Scalar(0));
840 Mat B(inPoints.size(),1,CV_32FC1, Scalar(0));
845 if( maxX-minX > maxY-minY ) {
847 for (
size_t i=0; i<inPoints.size(); ++i) {
849 A.at<
float>(i, 0) = inPoints[i].
x;
850 A.at<
float>(i, 1) = 1.;
851 B.at<
float>(i, 0) = inPoints[i].
y;
856 solve(A,B,X, DECOMP_SVD);
858 outLine = Point3f(X.at<
float>(0,0), -1., X.at<
float>(1,0));
862 for (
size_t i=0; i<inPoints.size(); ++i) {
864 A.at<
float>(i, 0) = inPoints[i].
y;
865 A.at<
float>(i, 1) = 1.;
866 B.at<
float>(i, 0) = inPoints[i].
x;
871 solve(A,B,X, DECOMP_SVD);
873 outLine = Point3f(-1., X.at<
float>(0,0), X.at<
float>(1,0));
880 Point2f MarkerDetector::getCrossPoint(
const cv::Point3f& line1,
const cv::Point3f& line2)
884 Mat A(2,2,CV_32FC1, Scalar(0));
885 Mat B(2,1,CV_32FC1, Scalar(0));
888 A.at<
float>(0, 0) = line1.x;
889 A.at<
float>(0, 1) = line1.y;
890 B.at<
float>(0, 0) = -line1.z;
892 A.at<
float>(1, 0) = line2.x;
893 A.at<
float>(1, 1) = line2.y;
894 B.at<
float>(1, 0) = -line2.z;
897 solve(A,B,X, DECOMP_SVD);
898 return Point2f(X.at<
float>(0,0), X.at<
float>(1,0));
915 void MarkerDetector::drawAllContours ( Mat input, std::vector<std::vector<cv::Point> > &contours )
917 drawContours ( input, contours, -1,Scalar ( 255,0,255 ) );
926 void MarkerDetector:: drawContour ( Mat &in,vector<Point> &contour,Scalar color )
928 for (
size_t i=0;i<contour.size();++i )
930 cv::rectangle ( in,contour[i],contour[i],color );
934 void MarkerDetector:: drawApproxCurve ( Mat &in,vector<Point> &contour,Scalar color )
936 for (
size_t i=0;i<contour.size();++i )
938 cv::line ( in,contour[i],contour[ ( i+1 ) %contour.size() ],color );
948 void MarkerDetector::draw ( Mat out,
const vector<Marker> &
markers )
950 for (
size_t i=0;i<markers.size();++i )
952 cv::line ( out,markers[i][0],markers[i][1],cv::Scalar ( 255,0,0 ),2,LINE_AA );
953 cv::line ( out,markers[i][1],markers[i][2],cv::Scalar ( 255,0,0 ),2,LINE_AA );
954 cv::line ( out,markers[i][2],markers[i][3],cv::Scalar ( 255,0,0 ),2,LINE_AA );
955 cv::line ( out,markers[i][3],markers[i][0],cv::Scalar ( 255,0,0 ),2,LINE_AA );
990 void MarkerDetector::glGetProjectionMatrix (
CameraParameters & CamMatrix,cv::Size orgImgSize, cv::Size size,
double proj_matrix[16],
double gnear,
double gfar,
bool invert )
991 throw ( cv::Exception )
993 cerr<<
"MarkerDetector::glGetProjectionMatrix . This a deprecated function. Use CameraParameters::glGetProjectionMatrix instead. "<<__FILE__<<
" "<<__LINE__<<endl;
994 CamMatrix.glGetProjectionMatrix ( orgImgSize,size,proj_matrix,gnear,gfar,invert );
1004 void MarkerDetector::setMinMaxSize(
float min ,
float max )
throw(cv::Exception)
1006 if (min<=0 || min>1)
throw cv::Exception(1,
" min parameter out of range",
"MarkerDetector::setMinMaxSize",__FILE__,__LINE__);
1007 if (max<=0 || max>1)
throw cv::Exception(1,
" max parameter out of range",
"MarkerDetector::setMinMaxSize",__FILE__,__LINE__);
1008 if (
min>max)
throw cv::Exception(1,
" min>max",
"MarkerDetector::setMinMaxSize",__FILE__,__LINE__);
void findCornerPointsInContour(const vector< cv::Point2f > &points, const vector< cv::Point > &contour, vector< int > &idxs)
int findDeformedSidesIdx(const vector< cv::Point > &contour, const vector< int > &idxSegments)
void setPointIntoImage(cv::Point &p, cv::Size s)
TFSIMD_FORCE_INLINE const tfScalar & y() const
TFSIMD_FORCE_INLINE Quaternion inverse(const Quaternion &q)
TFSIMD_FORCE_INLINE const tfScalar & x() const
Parameters of the camera.
TFSIMD_FORCE_INLINE const tfScalar & w() const
vector< cv::Point > contour
static int detect(const cv::Mat &in, int &nRotations)