00001
00005 #include "calibration.hpp"
00006
00007 void generateRandomIndexArray(int * randomArray, int maxElements, int maxVal)
00008 {
00009
00010 srand ( (unsigned int)(time(NULL)) );
00011
00012 vector<int> validValuesVector, randomSelection;
00013
00014 for (int iii = 0; iii < maxVal; iii++)
00015 {
00016 validValuesVector.push_back(iii);
00017 }
00018
00019 for (int iii = 0; iii < maxElements; iii++)
00020 {
00021 int currIndex = rand() % validValuesVector.size();
00022
00023 randomSelection.push_back(validValuesVector.at(currIndex));
00024 validValuesVector.erase(validValuesVector.begin() + currIndex);
00025 }
00026
00027 sort(randomSelection.begin(), randomSelection.end());
00028
00029 for (int iii = 0; iii < randomSelection.size(); iii++)
00030 {
00031 randomArray[iii] = randomSelection.at(iii);
00032 }
00033 }
00034
00035 mserPatch::mserPatch()
00036 {
00037
00038 }
00039
00040 mserPatch::mserPatch(vector<Point>& inputHull, const Mat& image)
00041 {
00042 double dbx, dby;
00043 int x, y, division = 0;
00044 meanIntensity = 0.0;
00045
00046 copyContour(inputHull, hull);
00047
00048
00049 momentSet = moments(Mat(hull));
00050
00051
00052 dbx = (momentSet.m10/momentSet.m00);
00053 dby = (momentSet.m01/momentSet.m00);
00054 x = (int) dbx;
00055 y = (int) dby;
00056 centroid = Point(x,y);
00057 centroid2f = Point2f(dbx, dby);
00058
00059
00060 area = contourArea(Mat(hull));
00061
00062 int searchDist = 15;
00063
00064
00065 for (int i = x-searchDist; i < x+searchDist+1; i++)
00066 {
00067 for (int j = y-searchDist; j < y+searchDist+1; j++)
00068 {
00069
00070 if (pointPolygonTest(Mat(hull), Point2f(i,j), false) > 0.0)
00071 {
00072
00073 meanIntensity = meanIntensity + double(image.at<Vec3b>(j,i)[0]);
00074 division++;
00075 }
00076 }
00077 }
00078
00079 meanIntensity /= division;
00080
00081
00082 varIntensity = 0.0;
00083
00084 for (int i = x-searchDist; i < x+searchDist+1; i++)
00085 {
00086 for (int j = y-searchDist; j < y+searchDist+1; j++)
00087 {
00088
00089 if (pointPolygonTest(Mat(hull), Point2f(i,j), false) > 0.0)
00090 {
00091
00092 varIntensity += pow((meanIntensity - double(image.at<Vec3b>(j,i)[0])), 2);
00093 }
00094 }
00095 }
00096
00097 varIntensity /= division;
00098
00099
00100
00101
00102
00103
00104
00105
00106
00107
00108
00109
00110
00111
00112
00113
00114
00115
00116
00117
00118
00119
00120
00121
00122
00123
00124
00125
00126
00127 }
00128
00129 bool findMaskCorners(const Mat& image, Size patternSize, vector<Point2f>& corners, int detector, int mserDelta, float max_var, float min_diversity, double area_threshold)
00130 {
00131 Mat grayIm;
00132 if (image.channels() > 1) {
00133 cvtColor(image, grayIm, CV_RGB2GRAY);
00134 } else {
00135 grayIm = Mat(image);
00136 }
00137
00138 return findPatternCorners(grayIm, patternSize, corners, 1, detector, mserDelta, max_var, min_diversity, area_threshold);
00139 }
00140
00141
00142 bool checkAcutance()
00143 {
00144 bool retVal = true;
00145
00146 return retVal;
00147 }
00148
00149 void determinePatchDistribution(Size patternSize, int mode, int &rows, int &cols, int &quant)
00150 {
00151 if (mode == 0)
00152 {
00153 rows = patternSize.height + 1;
00154 cols = patternSize.width + 1;
00155 quant = patternSize.width*patternSize.height/2;
00156 }
00157 else
00158 {
00159 rows = patternSize.height / 2;
00160 cols = patternSize.width / 2;
00161 quant = patternSize.width*patternSize.height/4;
00162 }
00163
00164 }
00165
00166 void findAllPatches(const Mat& image, Size patternSize, vector<vector<Point> >& msers, int delta, float max_var, float min_diversity, double area_threshold)
00167 {
00168
00169
00170
00171 int64 t = getTickCount();
00172
00173 int imWidth = image.size().width;
00174 int imHeight = image.size().height;
00175 int maxArea, minArea;
00176
00177 maxArea = 2 * (imWidth / (patternSize.width-1)) * (imHeight / (patternSize.height-1));
00178 minArea = MINIMUM_MSER_AREA;
00179
00180 int X = patternSize.width/2, Y = patternSize.height/2;
00181
00182 Mat displayMat(image);
00183 Mat mask = Mat::ones(image.rows, image.cols, CV_8U);
00184
00185
00186
00187
00188 MSER mserExtractor(delta, minArea, maxArea, max_var, min_diversity, 200, area_threshold, 0.003, 5);
00189
00190
00191
00192
00193
00194
00195
00196
00197
00198
00199
00200 Mat imGrey;
00201 if (image.channels() > 1)
00202 {
00203 cvtColor(image, imGrey, CV_RGB2GRAY);
00204 }
00205 else
00206 {
00207 image.copyTo(imGrey);
00208 }
00209
00210
00211 int kernelSize = min(imWidth, imHeight) / (5*max(patternSize.width, patternSize.height));
00212 kernelSize = kernelSize + 1 + (kernelSize % 2);
00213
00214 GaussianBlur(imGrey, imGrey, Size(kernelSize,kernelSize), 0,0);
00215
00216
00217
00218
00219
00220
00221
00222
00223
00224
00225
00226
00227
00228
00229 mserExtractor(imGrey, msers, mask);
00230
00231
00232
00233
00234 for (unsigned int i = 0; i < msers.size(); i++)
00235 {
00236 convexHull(Mat(msers[i]), msers[i]);
00237 }
00238
00239
00240
00241 if (DEBUG_MODE > 1)
00242 {
00243 printf("%s << MSERs found: %d\n", __FUNCTION__, msers.size());
00244 }
00245
00246 if (DEBUG_MODE > 0)
00247 {
00248 t = getTickCount() - t;
00249 printf("%s << Algorithm duration: %fms\n", __FUNCTION__, t*1000/getTickFrequency());
00250 }
00251
00252
00253
00254
00255
00256
00257
00258
00259
00260 if (DEBUG_MODE > 1)
00261 {
00262 printf("%s << Total patches found = %d\n", __FUNCTION__, msers.size());
00263 }
00264 }
00265
00266 void randomCulling(vector<std::string> &inputList, int maxSearch)
00267 {
00268
00269 srand ( (unsigned int)(time(NULL)) );
00270
00271 int deletionIndex = 0;
00272
00273 while (inputList.size() > maxSearch)
00274 {
00275 deletionIndex = rand() % inputList.size();
00276 inputList.erase(inputList.begin() + deletionIndex);
00277 }
00278 }
00279
00280 void randomCulling(vector<std::string> &inputList, int maxSearch, vector<vector<Point2f> >& patterns)
00281 {
00282
00283 srand ( (unsigned int)(time(NULL)) );
00284
00285 int deletionIndex = 0;
00286
00287 while (inputList.size() > maxSearch)
00288 {
00289 deletionIndex = rand() % inputList.size();
00290 inputList.erase(inputList.begin() + deletionIndex);
00291 patterns.erase(patterns.begin() + deletionIndex);
00292 }
00293 }
00294
00295 void randomCulling(vector<string>& inputList, int maxSearch, vector<vector<vector<Point2f> > >& patterns)
00296 {
00297 srand ( (unsigned int)(time(NULL)) );
00298
00299 int deletionIndex = 0;
00300
00301 printf("%s << inputList.size() = %d / %d\n", __FUNCTION__, inputList.size(), maxSearch);
00302
00303 while (inputList.size() > maxSearch)
00304 {
00305 deletionIndex = rand() % inputList.size();
00306 inputList.erase(inputList.begin() + deletionIndex);
00307
00308 for (int i = 0; i < patterns.size(); i++)
00309 {
00310 patterns.at(i).erase(patterns.at(i).begin() + deletionIndex);
00311 }
00312
00313 }
00314
00315
00316
00317
00318 }
00319
00320 void debugDisplayPatches(const Mat& image, vector<vector<Point> >& msers)
00321 {
00322
00323 Scalar color(0, 0, 255);
00324 Mat dispMat;
00325
00326 image.copyTo(dispMat);
00327
00328 drawContours(dispMat, msers, -1, color, 1);
00329
00330 if (image.cols > 640)
00331 {
00332 Mat dispMat2;
00333 resize(dispMat, dispMat2, Size(0,0), 0.5, 0.5);
00334 imshow("mainWin", dispMat2);
00335 }
00336 else
00337 {
00338 imshow("mainWin", dispMat);
00339 }
00340
00341 waitKey(0);
00342 }
00343
00344 void determineFindablePatches(Size patternSize, int mode, int *XVec, int *YVec)
00345 {
00346 int X, Y;
00347
00348 if (mode == 0)
00349 {
00350
00351 Y = patternSize.height + 1;
00352
00353
00354
00355 for (int i = 0; i < Y; i++)
00356 {
00357 if ((i % 2) > 0)
00358 {
00359 XVec[i] = int(ceil(((double(patternSize.width) + 1)/2)-0.01)-1);
00360 }
00361 else
00362 {
00363 XVec[i] = int(floor(((double(patternSize.width) + 1)/2)+0.01)-1);
00364 }
00365
00366 }
00367
00368
00369 YVec[0] = int(floor(((patternSize.height + 1)/2)+0.01)-1);
00370
00371 if (patternSize.width % 2)
00372 {
00373 if (patternSize.height % 2)
00374 {
00375 YVec[1] = int(floor(((patternSize.height + 1)/2)+0.01)-1);
00376 }
00377 else
00378 {
00379 YVec[1] = int(floor(((patternSize.height + 1)/2)+0.01));
00380 }
00381
00382 }
00383 else
00384 {
00385 YVec[1] = int(floor(((patternSize.height + 1)/2)+0.01)-1);
00386 }
00387
00388 }
00389 else
00390 {
00391 X = patternSize.width/2;
00392 Y = patternSize.height/2;
00393
00394 for (int i = 0; i < Y; i++)
00395 {
00396 if (i % 2)
00397 {
00398 XVec[i] = patternSize.width/2 - 2;
00399 }
00400 else
00401 {
00402 XVec[i] = patternSize.width/2 - 2;
00403 }
00404
00405 }
00406
00407 YVec[0] = Y-2;
00408 YVec[1] = Y-2;
00409 }
00410
00411 if (DEBUG_MODE > 3)
00412 {
00413 printf("%s << Total number of rows: %d\n", __FUNCTION__, Y);
00414 printf("%s << Number of findable patches in LHS: %d\n", __FUNCTION__, YVec[0]);
00415 printf("%s << Number of findable patches in RHS: %d\n", __FUNCTION__, YVec[1]);
00416
00417 for (int i = 0; i < Y; i++)
00418 {
00419 printf("%s << Number of findable patches in row[%d]: %d\n", __FUNCTION__, i, XVec[i]);
00420 }
00421 }
00422 }
00423
00424 void findCornerPatches(Size imageSize, Size patternSize, int mode, int *XVec, int *YVec, vector<Point2f>& patchCentres, vector<Point2f>& remainingPatches)
00425 {
00426
00427 Point2f center;
00428 float angle;
00429 Size2f size;
00430 vector<vector<Point> > sideCenters(1);
00431 vector<vector<Point> > fourCorners(1);
00432 vector<Point> orderedCorners;
00433 vector<vector<Point> > orderedForDisplay;
00434 double x,y,X,Y;
00435
00436
00437
00438
00439 RotatedRect wrapperRectangle;
00440
00441 wrapperRectangle = fitEllipse(Mat(remainingPatches));
00442
00443 center = wrapperRectangle.center;
00444 angle = wrapperRectangle.angle;
00445 size = wrapperRectangle.size;
00446
00447
00448
00449
00450
00451
00452 x = center.x;
00453 y = center.y;
00454 X = (size.width / 2) * 1.2;
00455 Y = (size.height / 2) * 1.2;
00456
00457
00458
00459
00460
00461
00462
00463
00464
00465
00466 fourCorners.at(0).push_back(Point(int(x-X*cos((3.14/180)*angle)-Y*sin((3.14/180)*angle)), int(y-X*sin((3.14/180)*angle)+Y*cos((3.14/180)*angle))));
00467 fourCorners.at(0).push_back(Point(int(x+Y*sin((3.14/180)*angle)-X*cos((3.14/180)*angle)), int(y-Y*cos((3.14/180)*angle)-X*sin((3.14/180)*angle))));
00468 fourCorners.at(0).push_back(Point(int(x+X*cos((3.14/180)*angle)+Y*sin((3.14/180)*angle)), int(y+X*sin((3.14/180)*angle)-Y*cos((3.14/180)*angle))));
00469 fourCorners.at(0).push_back(Point(int(x-Y*sin((3.14/180)*angle)+X*cos((3.14/180)*angle)), int(y+Y*cos((3.14/180)*angle)+X*sin((3.14/180)*angle))));
00470
00471
00472 for (int i = 0; i < 4; i++)
00473 {
00474
00475 }
00476
00477
00478
00479 Mat imCpy(imageSize, CV_8UC3);
00480 imCpy.setTo(0);
00481 Scalar color( rand()&255, rand()&255, rand()&255 );
00482
00483 ellipse(imCpy, wrapperRectangle, color);
00484
00485 color = Scalar(rand()&255, rand()&255, rand()&255);
00486
00487
00488
00489
00490
00491
00492
00493
00494
00495
00496 double distMin[4] = {99999999, 99999999, 99999999, 99999999};
00497 double dist;
00498 int distIndex[4] = {0, 0, 0, 0};
00499 float extremesX[4] = {0.0, float(imageSize.width), float(imageSize.width), 0.0};
00500 float extremesY[4] = {0.0, 0.0, float(imageSize.height), float(imageSize.height)};
00501 Point tmpPoint;
00502 double minDist;
00503 int minIndex = 0;
00504 double sumDist;
00505
00506
00507
00508 minDist = 9e9;
00509
00510 for (int k = 0; k < 4; k++)
00511 {
00512 orderedCorners.clear();
00513 sumDist = 0;
00514
00515 for (int i = 0; i < 4; i++)
00516 {
00517 tmpPoint = Point(int(extremesX[i]), int(extremesY[i]));
00518 dist = distBetweenPts(fourCorners.at(0).at((k+i)%4), tmpPoint);
00519
00520
00521 sumDist += dist;
00522 }
00523
00524
00525
00526 if (sumDist < minDist)
00527 {
00528 minDist = sumDist;
00529 minIndex = k;
00530 }
00531 }
00532
00533
00534
00535
00536 orderedCorners.push_back(fourCorners.at(0).at((minIndex)%4));
00537 orderedCorners.push_back(fourCorners.at(0).at((minIndex+1)%4));
00538 orderedCorners.push_back(fourCorners.at(0).at((minIndex+3)%4));
00539 orderedCorners.push_back(fourCorners.at(0).at((minIndex+2)%4));
00540
00541 for (int i = 0; i < 4; i++)
00542 {
00543
00544 }
00545
00546
00547
00548
00549
00550
00551
00552
00553
00554
00555
00556 vector<Point> pointPair;
00557
00558
00559 for (int k = 0; k < 4; k++)
00560 {
00561 minDist = 9e9;
00562 for (unsigned int i = 0; i < remainingPatches.size(); i++)
00563 {
00564
00565
00566
00567 pointPair.clear();
00568 pointPair.push_back(orderedCorners.at(k));
00569 pointPair.push_back(remainingPatches.at(i));
00570 dist = arcLength(Mat(pointPair), false);
00571
00572
00573
00574 if (dist < minDist)
00575 {
00576
00577 minDist = dist;
00578 minIndex = i;
00579 }
00580
00581 }
00582
00583
00584
00585 transferElement(patchCentres, remainingPatches, minIndex);
00586 }
00587
00588 if (DEBUG_MODE > 3)
00589 {
00590 for (unsigned int i = 0; i < patchCentres.size(); i++)
00591 {
00592 printf("%s << patchCentres.at(%d) = (%f, %f)\n", __FUNCTION__, i, patchCentres.at(i).x, patchCentres.at(i).y);
00593 }
00594
00595 printf("%s << remainingPatches.size() = %d\n", __FUNCTION__, remainingPatches.size());
00596 }
00597
00598 return;
00599
00600
00601
00602
00603
00604
00605
00606
00607
00608
00609
00610
00611
00612
00613
00614
00615
00616
00617
00618
00619
00620
00621 if (DEBUG_MODE > 3)
00622 {
00623 printf("%s << imageSize.width = %d; imageSize.height = %d\n", __FUNCTION__, imageSize.width, imageSize.height);
00624 }
00625
00626
00627
00628
00629 Point intermediatePt;
00630
00631 Point tempPt;
00632
00633
00634
00635 if (DEBUG_MODE > 3)
00636 {
00637 printf("%s << remainingPatches.size() = %d\n", __FUNCTION__, remainingPatches.size());
00638 }
00639
00640
00641 for (int k = 0; k < 4; k++)
00642 {
00643 minDist = 99999999;
00644
00645 for (unsigned int i = 0; i < remainingPatches.size(); i++)
00646 {
00647
00648
00649
00650 pointPair.clear();
00651 pointPair.push_back(Point(int(extremesX[k]), int(extremesY[k])));
00652 pointPair.push_back(remainingPatches.at(i));
00653 dist = arcLength(Mat(pointPair), false);
00654
00655
00656
00657 if (dist < minDist)
00658 {
00659
00660 minDist = dist;
00661 minIndex = i;
00662 }
00663
00664 }
00665
00666
00667
00668 transferElement(patchCentres, remainingPatches, minIndex);
00669
00670
00671
00672 }
00673
00674 if (DEBUG_MODE > 3)
00675 {
00676 for (unsigned int i = 0; i < patchCentres.size(); i++)
00677 {
00678 printf("%s << patchCentres.at(%d) = (%f, %f)\n", __FUNCTION__, i, patchCentres.at(i).x, patchCentres.at(i).y);
00679 }
00680
00681 printf("%s << remainingPatches.size() = %d\n", __FUNCTION__, remainingPatches.size());
00682 }
00683
00684
00685 }
00686
00687 void findEdgePatches(Size patternSize, int mode, int *XVec, int *YVec, vector<Point2f>& patchCentres, vector<Point2f>& remainingPatches)
00688 {
00689 int minDist, minIndex, sortedIndex = patchCentres.size();
00690 double dist;
00691 vector<Point2f> patchString;
00692 Point2f interPoint;
00693
00694
00695
00696
00697
00698 for (int k = 0; k < YVec[0]; k++)
00699 {
00700
00701 minDist = 99999999;
00702
00703 for (unsigned int i = 0; i < remainingPatches.size(); i++)
00704 {
00705
00706 dist = perpDist(patchCentres.at(0), patchCentres.at(2), remainingPatches.at(i));
00707
00708 if (dist < minDist)
00709 {
00710 minDist = int(dist);
00711 minIndex = i;
00712 }
00713 }
00714
00715
00716
00717 transferElement(patchString, remainingPatches, minIndex);
00718 }
00719
00720
00721
00722
00723
00724
00725 for (int k = 0; k < YVec[0]; k++)
00726 {
00727
00728 minDist = 99999999;
00729
00730 for (int i = k; i < YVec[0]; i++)
00731 {
00732
00733 dist = distBetweenPts2f(patchCentres.at(0), patchString.at(i));
00734
00735 if (dist < minDist)
00736 {
00737 minDist = int(dist);
00738 minIndex = i;
00739 }
00740 }
00741
00742
00743
00744 swapElements(patchString, k, minIndex);
00745
00746
00747 }
00748
00749
00750
00751
00752 if (mode == 0)
00753 {
00754
00755 interPoint = meanPoint(patchCentres.at(0), patchString.at(0));
00756 patchCentres.push_back(interPoint);
00757
00758
00759 for (int k = 0; k < YVec[0]-1; k++)
00760 {
00761
00762 interPoint = meanPoint(patchString.at(0), patchString.at(1));
00763 transferElement(patchCentres, patchString, 0);
00764 patchCentres.push_back(interPoint);
00765
00766 }
00767
00768
00769
00770
00771 interPoint = meanPoint(patchCentres.at(2), patchString.at(0));
00772 transferElement(patchCentres, patchString, 0);
00773 patchCentres.push_back(interPoint);
00774
00775 }
00776 else
00777 {
00778 for (int k = 0; k < YVec[0]; k++)
00779 {
00780
00781
00782 transferElement(patchCentres, patchString, 0);
00783 }
00784
00785 }
00786
00787
00788
00789 sortedIndex += YVec[0];
00790
00791
00792
00793
00794
00795 for (int k = 0; k < YVec[1]; k++)
00796 {
00797
00798 minDist = 99999999;
00799
00800 for (unsigned int i = 0; i < remainingPatches.size(); i++)
00801 {
00802
00803 dist = perpDist(patchCentres.at(1), patchCentres.at(3), remainingPatches.at(i));
00804
00805 if (dist < minDist)
00806 {
00807 minDist = int(dist);
00808 minIndex = i;
00809 }
00810 }
00811
00812
00813
00814 transferElement(patchString, remainingPatches, minIndex);
00815 }
00816
00817
00818
00819
00820 for (int k = 0; k < YVec[1]; k++)
00821 {
00822
00823 minDist = 99999999;
00824
00825 for (int i = k; i < YVec[1]; i++)
00826 {
00827
00828
00829
00830
00831 dist = distBetweenPts2f(patchCentres.at(1), patchString.at(i));
00832
00833 if (dist < minDist)
00834 {
00835 minDist = int(dist);
00836 minIndex = i;
00837 }
00838 }
00839
00840
00841
00842 swapElements(patchString, k, minIndex);
00843 }
00844
00845
00846
00847 if (mode == 0)
00848 {
00849
00850 interPoint = meanPoint(patchCentres.at(1), patchString.at(0));
00851 patchCentres.push_back(interPoint);
00852
00853
00854 for (int k = 0; k < YVec[1]-1; k++)
00855 {
00856
00857 interPoint = meanPoint(patchString.at(0), patchString.at(1));
00858 transferElement(patchCentres, patchString, 0);
00859 patchCentres.push_back(interPoint);
00860
00861 }
00862
00863
00864
00865
00866 interPoint = meanPoint(patchCentres.at(3), patchString.at(0));
00867 transferElement(patchCentres, patchString, 0);
00868 patchCentres.push_back(interPoint);
00869
00870 }
00871 else
00872 {
00873 for (int k = 0; k < YVec[1]; k++)
00874 {
00875
00876
00877 transferElement(patchCentres, patchString, 0);
00878 }
00879
00880 }
00881
00882 sortedIndex += YVec[1];
00883
00884
00885
00886 if (DEBUG_MODE > 3)
00887 {
00888
00889 printf("%s << patchCentres.size() = %d\n", __FUNCTION__, patchCentres.size());
00890 printf("%s << remainingPatches.size() = %d\n", __FUNCTION__, remainingPatches.size());
00891
00892 for (unsigned int i = 0; i < patchCentres.size(); i++)
00893 {
00894 printf("%s << patchCentres.at(%d) = (%f, %f)\n", __FUNCTION__, i, patchCentres.at(i).x, patchCentres.at(i).y);
00895 }
00896
00897
00898 }
00899 }
00900
00901 void findInteriorPatches(Size patternSize, int mode, int *XVec, int *YVec, vector<Point2f>& patchCentres, vector<Point2f>& remainingPatches)
00902 {
00903
00904
00905 int minDist, minIndex = 0, sortedIndex = patchCentres.size();;
00906 double dist;
00907
00908 vector<Point2f> patchString;
00909 Point2f interPoint;
00910
00911 int nRows;
00912
00913 if (mode == 0)
00914 {
00915 nRows = patternSize.height + 1;
00916 }
00917 else
00918 {
00919 nRows = patternSize.height / 2;
00920 }
00921
00922
00923
00924
00925 for (int r = 0; r < nRows; r++)
00926 {
00927
00928 patchString.clear();
00929
00930
00931
00932
00933 for (int k = 0; k < XVec[r]; k++)
00934 {
00935 minDist = 99999999;
00936
00937
00938
00939 for (unsigned int i = 0; i < remainingPatches.size(); i++)
00940 {
00941 if (r == 0)
00942 {
00943 dist = perpDist(patchCentres.at(0), patchCentres.at(1), remainingPatches.at(i));
00944 }
00945 else if (r == (nRows-1))
00946 {
00947 dist = perpDist(patchCentres.at(2), patchCentres.at(3), remainingPatches.at(i));
00948 }
00949 else
00950 {
00951 dist = perpDist(patchCentres.at(4+r-1), patchCentres.at(4+nRows-2+r-1), remainingPatches.at(i));
00952 }
00953
00954 if (dist < minDist)
00955 {
00956 minDist = int(dist);
00957 minIndex = i;
00958 }
00959
00960 }
00961
00962 transferElement(patchString, remainingPatches, minIndex);
00963 }
00964
00965
00966
00967
00968 for (int k = 0; k < XVec[r]; k++)
00969 {
00970 minDist = 99999999;
00971
00972 for (int i = k; i < XVec[r]; i++)
00973 {
00974 if (r == 0)
00975 {
00976 dist = distBetweenPts2f(patchCentres.at(0), patchString.at(i));
00977 }
00978 else if (r == (nRows-1))
00979 {
00980 dist = distBetweenPts2f(patchCentres.at(2), patchString.at(i));
00981 }
00982 else
00983 {
00984 dist = distBetweenPts2f(patchCentres.at(4+r-1), patchString.at(i));
00985 }
00986
00987
00988
00989 if (dist < minDist)
00990 {
00991 minDist = int(dist);
00992 minIndex = i;
00993 }
00994 }
00995
00996
00997
00998 swapElements(patchString, k, minIndex);
00999 }
01000
01001 for (int k = 0; k < XVec[r]; k++)
01002 {
01003 minDist = 99999999;
01004
01005 dist = distBetweenPts2f(patchCentres.at(0), patchString.at(k));
01006
01007
01008
01009 }
01010
01011
01012
01013 if (mode == 0)
01014 {
01015
01016 if (r == 0)
01017 {
01018 interPoint = meanPoint(patchCentres.at(0), patchString.at(0));
01019 patchCentres.push_back(interPoint);
01020 }
01021 else if (r == nRows-1)
01022 {
01023 interPoint = meanPoint(patchCentres.at(2), patchString.at(0));
01024 patchCentres.push_back(interPoint);
01025
01026 }
01027 else
01028 {
01029 if (r % 2)
01030 {
01031
01032 }
01033 else
01034 {
01035 interPoint = meanPoint(patchCentres.at(4+r-1), patchString.at(0));
01036 patchCentres.push_back(interPoint);
01037 }
01038
01039 }
01040
01041
01042 for (int k = 0; k < XVec[r]-1; k++)
01043 {
01044 interPoint = meanPoint(patchString.at(0), patchString.at(1));
01045 transferElement(patchCentres, patchString, 0);
01046 patchCentres.push_back(interPoint);
01047
01048 }
01049
01050
01051
01052
01053
01054
01055 if (r == 0)
01056 {
01057 interPoint = meanPoint(patchCentres.at(1), patchString.at(0));
01058 transferElement(patchCentres, patchString, 0);
01059 patchCentres.push_back(interPoint);
01060 }
01061 else if (r == (nRows-1))
01062 {
01063 interPoint = meanPoint(patchCentres.at(3), patchString.at(0));
01064 transferElement(patchCentres, patchString, 0);
01065 patchCentres.push_back(interPoint);
01066 }
01067 else
01068 {
01069 if (r % 2)
01070 {
01071 transferElement(patchCentres, patchString, 0);
01072 }
01073 else
01074 {
01075 interPoint = meanPoint(patchCentres.at(4+r-1+nRows-2), patchString.at(0));
01076 transferElement(patchCentres, patchString, 0);
01077 patchCentres.push_back(interPoint);
01078 }
01079
01080 }
01081
01082 }
01083 else
01084 {
01085 for (int k = 0; k < XVec[r]; k++)
01086 {
01087
01088 transferElement(patchCentres, patchString, 0);
01089 }
01090
01091 }
01092 }
01093
01094
01095
01096
01097
01098
01099
01100
01101
01102 if (DEBUG_MODE > 3)
01103 {
01104
01105 printf("%s << patchCentres.size() = %d\n", __FUNCTION__, patchCentres.size());
01106 printf("%s << remainingPatches.size() = %d\n", __FUNCTION__, remainingPatches.size());
01107
01108 for (unsigned int i = 0; i < patchCentres.size(); i++)
01109 {
01110
01111 }
01112
01113
01114 }
01115
01116 }
01117
01118 void sortPatches(Size imageSize, Size patternSize, vector<Point2f>& patchCentres, int mode)
01119 {
01120
01121
01122
01123 int desiredPatchCount, X, Y;
01124
01125 if (mode == 0)
01126 {
01127 desiredPatchCount = (patternSize.width + 1)*(patternSize.height + 1);
01128 }
01129 else
01130 {
01131 desiredPatchCount = (patternSize.width / 2)*(patternSize.height / 2);
01132 }
01133
01134 if (DEBUG_MODE > 3)
01135 {
01136 printf("%s << Looking for [%d] patches..\n", __FUNCTION__, desiredPatchCount);
01137
01138 }
01139
01140
01141 int *XVec;
01142 int YVec[2];
01143
01144
01145 if (mode == 0)
01146 {
01147 Y = patternSize.height + 1;
01148 XVec = new int[Y];
01149 }
01150 else
01151 {
01152 X = patternSize.width/2;
01153 Y = patternSize.height/2;
01154 XVec = new int[Y];
01155 }
01156
01157 determineFindablePatches(patternSize, mode, XVec, YVec);
01158
01159
01160 vector<Point2f> remainingPatches;
01161 remainingPatches.clear();
01162 patchCentres.swap(remainingPatches);
01163
01164
01165 findCornerPatches(imageSize, patternSize, mode, XVec, YVec, patchCentres, remainingPatches);
01166
01167
01168 findEdgePatches(patternSize, mode, XVec, YVec, patchCentres, remainingPatches);
01169
01170
01171 findInteriorPatches(patternSize, mode, XVec, YVec, patchCentres, remainingPatches);
01172
01173
01174 if (remainingPatches.size() > 0)
01175 {
01176 printf("%s << ERROR. There are remaining patches when there shouldn't be.\n", __FUNCTION__);
01177
01178
01179
01180
01181
01182 }
01183
01184 if (patchCentres.size() != desiredPatchCount)
01185 {
01186 printf("%s << ERROR. An incorrect number (%d vs %d) of patches have been allocated.\n", __FUNCTION__, patchCentres.size(), desiredPatchCount);
01187
01188
01189
01190
01191 while (patchCentres.size() < (unsigned int)(desiredPatchCount))
01192 {
01193 patchCentres.push_back(Point(0,0));
01194 }
01195
01196 delete[] XVec;
01197
01198 return;
01199 }
01200
01201
01202
01203 reorderPatches(patternSize, mode, XVec, YVec, patchCentres);
01204
01205 delete[] XVec;
01206 }
01207
01208 void reorderPatches(Size patternSize, int mode, int *XVec, int *YVec, vector<Point2f>& patchCentres)
01209 {
01210
01211
01212
01213
01214
01215
01216 vector<Point2f> reSortedCorners;
01217
01218 int nRows, nCols, nOffset;
01219
01220
01221
01222 if (mode == 0)
01223 {
01224 nRows = patternSize.height + 1;
01225 nCols = patternSize.width + 1;
01226 }
01227 else
01228 {
01229 nRows = patternSize.height / 2;
01230 nCols = patternSize.width / 2;
01231 }
01232
01233
01234
01235 nOffset = 4+2*(nRows-2);
01236
01237
01238
01239
01240 for (int r = 0; r < nRows; r++)
01241 {
01242
01243 if (r == 0)
01244 {
01245 reSortedCorners.push_back(patchCentres.at(0));
01246 }
01247 else if (r == (nRows-1))
01248 {
01249 reSortedCorners.push_back(patchCentres.at(2));
01250 }
01251 else
01252 {
01253 reSortedCorners.push_back(patchCentres.at(4+r-1));
01254 }
01255
01256
01257 for (int i = 0; i < nCols-2; i++)
01258 {
01259
01260 reSortedCorners.push_back(patchCentres.at(nOffset + r*(nCols-2) + i));
01261 }
01262
01263
01264
01265 if (r == 0)
01266 {
01267 reSortedCorners.push_back(patchCentres.at(1));
01268 }
01269 else if (r == (nRows-1))
01270 {
01271 reSortedCorners.push_back(patchCentres.at(3));
01272 }
01273 else
01274 {
01275 reSortedCorners.push_back(patchCentres.at(4+nRows-2+r-1));
01276 }
01277 }
01278
01279 reSortedCorners.swap(patchCentres);
01280
01281 if (DEBUG_MODE > 3)
01282 {
01283
01284 printf("%s << patchCentres.size() = %d\n", __FUNCTION__, patchCentres.size());
01285
01286 for (unsigned int i = 0; i < patchCentres.size(); i++)
01287 {
01288
01289 }
01290
01291
01292 }
01293
01294 }
01295
01296 void debugDisplayPattern(const Mat& image, Size patternSize, Mat& corners, bool mode, double delay)
01297 {
01298
01299 Mat tmpMat, dispMat;
01300
01301 mode = false;
01302
01303 if (image.channels() > 1)
01304 {
01305 cvtColor(image, tmpMat, CV_RGB2GRAY);
01306 }
01307 else
01308 {
01309 image.copyTo(tmpMat);
01310 }
01311
01312 cvtColor(tmpMat, dispMat, CV_GRAY2RGB);
01313
01314
01315
01316 drawChessboardCorners(dispMat, patternSize, corners, mode);
01317
01318 if (image.cols > 640)
01319 {
01320 Mat dispMat2;
01321 resize(dispMat, dispMat2, Size(0,0), 0.5, 0.5);
01322 imshow("mainWin", dispMat2);
01323 }
01324 else
01325 {
01326 imshow("mainWin", dispMat);
01327 }
01328 waitKey(delay);
01329 }
01330
01331 bool findPatternCorners(const Mat& image, Size patternSize, vector<Point2f>& corners, int mode, int detector, int mserDelta, float max_var, float min_div, double area_threshold)
01332 {
01333
01334
01335
01336 if (!checkAcutance())
01337 {
01338 return false;
01339 }
01340
01341 int patchCols, patchRows, desiredPatchQuantity;
01342 determinePatchDistribution(patternSize, mode, patchRows, patchCols, desiredPatchQuantity);
01343
01344 vector<vector<Point> > msers;
01345
01346 findAllPatches(image, patternSize, msers, mserDelta, max_var, min_div, area_threshold);
01347
01348 if (DEBUG_MODE > 2)
01349 {
01350 debugDisplayPatches(image, msers);
01351 }
01352
01353 if (msers.size() < desiredPatchQuantity)
01354 {
01355 corners.clear();
01356 if (DEBUG_MODE > 1)
01357 {
01358 printf("%s << Insufficient patches found. Returning.\n", __FUNCTION__);
01359 }
01360
01361 return false;
01362 }
01363
01364
01365
01366 vector<Point2f> patchCentres2f;
01367 bool found = refinePatches(image, patternSize, msers, patchCentres2f, mode);
01368
01369
01370 if (DEBUG_MODE > 1)
01371 {
01372 printf("%s << Patches found after refinement = %d\n", __FUNCTION__, msers.size());
01373 }
01374
01375 if (DEBUG_MODE > 2)
01376 {
01377 debugDisplayPatches(image, msers);
01378 }
01379
01380
01381
01382
01383 if (!found)
01384 {
01385 corners.clear();
01386
01387 if (DEBUG_MODE > 1)
01388 {
01389 printf("%s << Correct number of patches not found. Returning.\n", __FUNCTION__);
01390 }
01391
01392 return false;
01393 }
01394
01395
01396 found = patternInFrame(image.size(), patchCentres2f);
01397
01398
01399 if (!found)
01400 {
01401 corners.clear();
01402
01403 if (DEBUG_MODE > 1)
01404 {
01405 printf("%s << Some patch centres out of range...\n", __FUNCTION__);
01406 }
01407
01408 return false;
01409 }
01410
01411 sortPatches(image.size(), patternSize, patchCentres2f, mode);
01412
01413 if (DEBUG_MODE > 2)
01414 {
01415 Mat patchCentres_(patchCentres2f);
01416 debugDisplayPattern(image, cvSize(patchCols, patchRows), patchCentres_);
01417 }
01418
01419 found = verifyPatches(image.size(), patternSize, patchCentres2f, mode, MIN_CORNER_SEPARATION, MAX_CORNER_SEPARATION);
01420
01421 if (!found)
01422 {
01423 corners.clear();
01424
01425 if (DEBUG_MODE > 1)
01426 {
01427 printf("%s << Pattern verification failed. Returning.\n", __FUNCTION__);
01428 }
01429
01430 return false;
01431 }
01432
01433
01434 correctPatchCentres(image, patternSize, patchCentres2f, mode);
01435
01436 if (DEBUG_MODE > 2)
01437 {
01438 Mat patchCentres_(patchCentres2f);
01439 debugDisplayPattern(image, cvSize(patchCols, patchRows), patchCentres_);
01440 }
01441
01442 Mat homography;
01443 found = findPatchCorners(image, patternSize, homography, corners, patchCentres2f, mode, detector);
01444
01445
01446
01447
01448 if (!found)
01449 {
01450 corners.clear();
01451
01452 if (DEBUG_MODE > 1)
01453 {
01454 printf("%s << Patch corner search failed. Returning.\n", __FUNCTION__);
01455 }
01456
01457 return false;
01458 }
01459
01460 found = patternInFrame(image.size(), patchCentres2f);
01461
01462
01463 if (!found)
01464 {
01465 corners.clear();
01466
01467 if (DEBUG_MODE > 1)
01468 {
01469 printf("%s << Some corners out of range...\n", __FUNCTION__);
01470 }
01471
01472 return false;
01473 }
01474
01475 if (DEBUG_MODE > 2) {
01476 Mat cornersMat(corners);
01477 debugDisplayPattern(image, cvSize(patternSize.width, patternSize.height), cornersMat, true, 1.0);
01478 }
01479
01480 return found;
01481 }
01482
01483
01484 void interpolateCornerLocations2(const Mat& image, int mode, Size patternSize, vector<Point2f>& vCentres, vector<Point2f>& vCorners)
01485 {
01486
01487 if (DEBUG_MODE > 2)
01488 {
01489 printf("%s << Entered function...\n", __FUNCTION__);
01490 }
01491
01492 double minDimension = findMinimumSeparation(vCentres);
01493
01494 int correctionDistance = int(minDimension) / 4;
01495
01496
01497
01498 Mat imGrey;
01499
01500 if (image.channels() > 1)
01501 {
01502 cvtColor(image, imGrey, CV_RGB2GRAY);
01503 }
01504 else
01505 {
01506 image.copyTo(imGrey);
01507 }
01508
01509 int X, Y;
01510
01511 X = patternSize.width/2;
01512 Y = patternSize.height/2;
01513
01514 vector<Point2f> fullCentroidGrid;
01515
01516
01517 for (int i = 0; i < Y; i++)
01518 {
01519 for (int j = 0; j < X; j++)
01520 {
01521 Point2f pt;
01522
01523 pt = Point2f(j, i);
01524 fullCentroidGrid.push_back(pt);
01525
01526 }
01527 }
01528
01529 vector<Point2f> fullCornerGrid;
01530
01531
01532 for (int i = 0; i < Y; i++)
01533 {
01534 for (int j = 0; j < X; j++)
01535 {
01536 for (int k = 0; k < 4; k++)
01537 {
01538 Point2f pt;
01539
01540 if (k == 0)
01541 {
01542 pt = Point2f(j-0.25, i-0.25);
01543 }
01544 else if (k == 1)
01545 {
01546 pt = Point2f(j+0.25, i-0.25);
01547 }
01548 else if (k == 2)
01549 {
01550 pt = Point2f(j-0.25, i+0.25);
01551 }
01552 else if (k == 3)
01553 {
01554 pt = Point2f(j+0.25, i+0.25);
01555 }
01556
01557 fullCornerGrid.push_back(pt);
01558 }
01559
01560 }
01561 }
01562
01563 vector<Point2f> patchNeighbourhood;
01564 vector<Point2f> patchArrangement;
01565 vector<Point2f> newCorners, bestCorners;
01566 vector<Point2f> cornerArrangement;
01567 Mat cornerLocs(2, 2, CV_32FC2);
01568 Point2f *neighbourhoodArray;
01569 Point2f* arrangementArray;
01570
01571 Mat homography;
01572
01573
01574
01575 int neighbourhoodCount = 0;
01576
01577 int index = 0;
01578
01579
01580 for (int i = 0; i < Y; i++)
01581 {
01582 for (int j = 0; j < X; j++)
01583 {
01584
01585 if (0)
01586 {
01587 for (int k = 0; k < 4; k++)
01588 {
01589 newCorners.push_back(Point2f(0.0, 0.0));
01590 index++;
01591 }
01592
01593 }
01594 else
01595 {
01596 patchNeighbourhood.clear();
01597 patchArrangement.clear();
01598
01599 neighbourhoodCount = 0;
01600
01601
01602 for (int k = max(0, i-1); k < min(Y, i+2); k++)
01603 {
01604 for (int l = max(0, j-1); l < min(X, j+2); l++)
01605 {
01606
01607 patchNeighbourhood.push_back(vCentres.at(k*X+l));
01608
01609 patchArrangement.push_back(Point2f((float)l, (float)k));
01610
01611 neighbourhoodCount++;
01612 }
01613 }
01614
01615
01616
01617
01618
01619
01620
01621
01622
01623
01624
01625
01626 homography = findHomography(Mat(patchArrangement), Mat(patchNeighbourhood));
01627
01628 cornerArrangement.clear();
01629
01630
01631 for (int k = 0; k < 2; k++)
01632 {
01633 for (int l = 0; l < 2; l++)
01634 {
01635 cornerArrangement.push_back(Point2f((float)(j-0.25+0.5*l), (float)(i-0.25+0.5*k)));
01636 }
01637 }
01638
01639
01640
01641 Mat tmpMat1 = Mat(cornerArrangement);
01642
01643 perspectiveTransform(tmpMat1, cornerLocs, homography);
01644
01645 for (int k = 0; k < 4; k++)
01646 {
01647
01648 newCorners.push_back(Point2f(cornerLocs.at<Vec3f>(k,0)[0], cornerLocs.at<Vec3f>(k,0)[1]));
01649 }
01650
01651 }
01652
01653
01654
01655
01656 }
01657 }
01658
01659
01660
01661
01662
01663
01664
01665
01666
01667
01668
01669
01670
01671 if (DEBUG_MODE > 2)
01672 {
01673 vector<Point2f> redistorted;
01674 Mat cornersForDisplay(newCorners);
01675 debugDisplayPattern(image, cvSize(patternSize.width, patternSize.height), cornersForDisplay);
01676 printf("%s << DONE.\n", __FUNCTION__);
01677 }
01678
01679 vCorners.clear();
01680 vCorners.assign(newCorners.begin(), newCorners.end());
01681
01682
01683 }
01684
01685 void addToBinMap(Mat& binMap, cv::vector<Point2f>& cornerSet, Size imSize)
01686 {
01687 int x, y;
01688 for (unsigned int i = 0; i < cornerSet.size(); i++)
01689 {
01690
01691 x = (int)((cornerSet.at(i).x/(double(imSize.width)))*binMap.cols);
01692 y = (int)((cornerSet.at(i).y/(double(imSize.height)))*binMap.rows);
01693
01694
01695 binMap.at<int>(y, x) += 1;
01696 }
01697 }
01698
01699 void prepForDisplay(const Mat& distributionMap, Mat& distributionDisplay)
01700 {
01701
01702 cv::Mat distMapCpy(distributionMap.size(), CV_8UC3), distMapCpyGray(distributionMap.size(), CV_8UC1);
01703 distributionDisplay = cv::Mat(distributionMap.size(), CV_8UC1);
01704
01705
01706
01707
01708 distMapCpy = cv::Mat::zeros(distributionMap.size(), CV_8UC3);
01709
01710 Scalar markerColor(0,0,0);
01711
01712 for (unsigned int iii = 0; iii < distributionMap.rows; iii++) {
01713 for (unsigned int jjj = 0; jjj < distributionMap.cols; jjj++) {
01714
01715
01716
01717 distMapCpy.at<Vec3b>(iii,jjj)[0] = 255;
01718 distMapCpy.at<Vec3b>(iii,jjj)[1] = 255;
01719 distMapCpy.at<Vec3b>(iii,jjj)[2] = 255;
01720
01721
01722
01723 if (distributionMap.at<unsigned char>(iii,jjj) > 0) {
01724 unsigned char val = 0;
01725 markerColor = Scalar(val,val,val);
01726 circle(distMapCpy, Point(jjj,iii), 0, markerColor, 5, CV_AA, 0);
01727 }
01728
01729
01730
01731
01732
01733 }
01734 }
01735
01736
01737
01738 cvtColor(distMapCpy, distMapCpyGray, CV_RGB2GRAY);
01739
01740
01741
01742 equalizeHist(distMapCpyGray, distributionDisplay);
01743
01744
01745
01746
01747 GaussianBlur(distributionDisplay, distMapCpyGray, Size(11, 11), 1.0, 1.0);
01748
01749
01750
01751
01752
01753 normalize(distMapCpyGray, distributionDisplay, 0,255, NORM_MINMAX);
01754
01755
01756
01757
01758
01759
01760 return;
01761 }
01762
01763 void addToRadialDistribution(double *radialDistribution, cv::vector<Point2f>& cornerSet, Size imSize)
01764 {
01765
01766 Point2f center((float)((double(imSize.height-1))/2), (float)((double(imSize.width-1))/2));
01767 double dist, maxDist;
01768 int index;
01769
01770
01771
01772
01773
01774 maxDist = pow(pow(double(imSize.height)/2, 2) + pow(double(imSize.width)/2, 2), 0.5);
01775
01776 for (unsigned int i = 0; i < cornerSet.size(); i++)
01777 {
01778
01779 dist = distBetweenPts2f(center, cornerSet.at(i));
01780
01781
01782 if (dist < maxDist)
01783 {
01784 index = int((dist/maxDist)*(RADIAL_LENGTH-0.00001));
01785 radialDistribution[index]++;
01786 }
01787
01788 }
01789
01790 }
01791
01792 void addToDistributionMap(Mat& distributionMap, vector<Point2f>& corners)
01793 {
01794 for (unsigned int i = 0; i < corners.size(); i++)
01795 {
01796 distributionMap.at<unsigned char>((int)corners.at(i).y, (int)corners.at(i).x) += (unsigned char) 1;
01797 }
01798 }
01799
01800 double obtainSetScore(Mat& distributionMap,
01801 Mat& binMap,
01802 Mat& gaussianMat,
01803 cv::vector<Point2f>& cornerSet,
01804 double *radialDistribution)
01805 {
01806 double score = 1.0;
01807 double mean = 0.0, variance = 0.0;
01808 int count = 0;
01809 double area = 0.0;
01810 Point centroid, center;
01811 double centrality = 0.0;
01812
01813 Mat distributionDisplay(distributionMap.size(), CV_8UC1);
01814 Mat binTemp(binMap.size(), CV_8UC1);
01815
01816 center = Point(distributionMap.size().width/2 - 1, distributionMap.size().height/2 - 1);
01817
01818 cv::vector<Point> fullHull, simplifiedHull;
01819
01820 for (unsigned int i = 0; i < cornerSet.size(); i++)
01821 {
01822 fullHull.push_back(Point(int(cornerSet.at(i).x), int(cornerSet.at(i).y)));
01823 }
01824
01825
01826 Mat binMapCpy;
01827 binMap.copyTo(binMapCpy);
01828
01829
01830 addToBinMap(binMapCpy, cornerSet, distributionMap.size());
01831
01832 count = binMapCpy.size().width * binMapCpy.size().height;
01833
01834
01835
01836 convexHull(Mat(fullHull), simplifiedHull);
01837
01838 area = contourArea(Mat(simplifiedHull));
01839
01840 area /= (distributionMap.size().width * distributionMap.size().height);
01841
01842
01843
01844
01845 centroid = findCentroid(simplifiedHull);
01846 centrality = distBetweenPts(centroid, center);
01847 centrality = 1 - (centrality / (distributionMap.size().height/2));
01848
01849
01850 for (int i = 0; i < binMapCpy.size().width; i++)
01851 {
01852 for (int j = 0; j < binMapCpy.size().height; j++)
01853 {
01854 mean += binMapCpy.at<int>(j, i);
01855 }
01856 }
01857
01858
01859
01860
01861
01862
01863 double *radialDistCpy;
01864 radialDistCpy = new double[RADIAL_LENGTH];
01865 for (int i = 0; i < RADIAL_LENGTH; i++)
01866 {
01867 radialDistCpy[i] = radialDistribution[i];
01868 }
01869
01870
01871 addToRadialDistribution(radialDistCpy, cornerSet, distributionMap.size());
01872
01873 double radialMean = 0.0, radialVariance = 0.0, desiredCount;
01874 double *radialCumulative;
01875 radialCumulative = new double[RADIAL_LENGTH];
01876
01877
01878
01879 for (int i = 0; i < RADIAL_LENGTH; i++)
01880 {
01881 radialMean += radialDistCpy[i];
01882 radialCumulative[i] = radialMean;
01883
01884 }
01885 radialMean /= RADIAL_LENGTH;
01886
01887
01888
01889
01890
01891 for (int i = 0; i < RADIAL_LENGTH; i++)
01892 {
01893 desiredCount = double(i+1)*radialMean;
01894
01895
01896 radialVariance += pow(radialCumulative[i] - desiredCount, 2);
01897
01898
01899
01900 }
01901
01902
01903
01904
01905
01906
01907
01908 radialVariance /= RADIAL_LENGTH;
01909 radialVariance = pow(radialVariance, 0.5);
01910
01911
01912
01913
01914 mean /= count;
01915
01916
01917
01918
01919
01920
01921 Mat distMapCpy(distributionMap.size(), CV_64FC1);
01922
01923 for (int i = 0; i < distMapCpy.size().width; i++)
01924 {
01925 for (int j = 0; j < distMapCpy.size().height; j++)
01926 {
01927 distMapCpy.at<double>(j,i) += double(distributionMap.at<unsigned char>(j,i));
01928 }
01929 }
01930
01931
01932
01933
01934
01935
01936
01937
01938
01939
01940
01941
01942
01943
01944
01945 gaussianMat *= mean;
01946
01947 for (int i = 0; i < binMapCpy.size().width; i++)
01948 {
01949 for (int j = 0; j < binMapCpy.size().height; j++)
01950 {
01951 variance += pow((double(binMapCpy.at<int>(j, i)) - gaussianMat.at<double>(j, i)), 2);
01952 }
01953 }
01954
01955
01956 gaussianMat /= mean;
01957 variance /= count;
01958
01959
01960
01961
01962
01963
01964
01965
01966
01967
01968
01969
01970
01971
01972
01973
01974
01975 score = pow(area, 2.0) / radialVariance;
01976
01977
01978
01979
01980 delete[] radialDistCpy;
01981 delete[] radialCumulative;
01982
01983 if (score < 0)
01984 {
01985 printf("%s << ERROR: Negative score acquired. Returning.\n", __FUNCTION__);
01986 return -1;
01987 }
01988
01989 return score;
01990 }
01991
01992 bool findPatchCorners(const Mat& image, Size patternSize, Mat& homography, vector<Point2f>& corners, vector<Point2f>& patchCentres2f, int mode, int detector)
01993 {
01994
01995 if (DEBUG_MODE > 2)
01996 {
01997 printf("%s << Entered function...\n", __FUNCTION__);
01998 }
01999
02000 Mat inputDisp;
02001
02002 vector<Point2f> cornerEstimates;
02003 vector<vector<Point2f> > vvOriginalCentres;
02004 vvOriginalCentres.push_back(patchCentres2f);
02005
02006 interpolateCornerLocations2(image, mode, patternSize, vvOriginalCentres.at(0), cornerEstimates);
02007
02008
02009
02010 if (DEBUG_MODE > 2)
02011 {
02012 Mat cornersForDisplay(cornerEstimates);
02013 printf("%s << Step 3: cornerEstimates.size() = %d\n", __FUNCTION__, cornerEstimates.size());
02014 debugDisplayPattern(image, cvSize(patternSize.width, patternSize.height), cornersForDisplay);
02015 printf("%s << DONE.\n", __FUNCTION__);
02016 }
02017
02018 corners.clear();
02019
02020
02021
02022
02023 sortCorners(image.size(), patternSize, cornerEstimates);
02024
02025 findBestCorners(image, cornerEstimates, corners, patternSize, detector);
02026
02027
02028
02029 if (DEBUG_MODE > 3)
02030 {
02031 Mat cornersForDisplay(corners);
02032 debugDisplayPattern(image, cvSize(patternSize.width, patternSize.height), cornersForDisplay);
02033 }
02034
02035 bool retVal = false;
02036
02037
02038
02039 retVal = verifyCorners(image.size(), patternSize, corners, MIN_CORNER_SEPARATION, MAX_CORNER_SEPARATION);
02040
02041 if (!retVal)
02042 {
02043 corners.clear();
02044 return retVal;
02045 }
02046
02047 groupPointsInQuads(patternSize, corners);
02048
02049 if (DEBUG_MODE > 3) {
02050 printf("%s << Displaying corners after grouping...\n", __FUNCTION__);
02051 Mat cornersForDisplay(corners);
02052 debugDisplayPattern(image, cvSize(patternSize.width, patternSize.height), cornersForDisplay, true, 50.0);
02053 }
02054
02055 refineCornerPositions(image, patternSize, corners);
02056
02057 if (DEBUG_MODE > 3) {
02058 printf("%s << Displaying corners after refinement...\n", __FUNCTION__);
02059 Mat cornersForDisplay(corners);
02060 debugDisplayPattern(image, cvSize(patternSize.width, patternSize.height), cornersForDisplay, true, 50.0);
02061 }
02062
02063 sortCorners(image.size(), patternSize, corners);
02064
02065 if (DEBUG_MODE > 3) {
02066 printf("%s << Displaying corners after sorting...\n", __FUNCTION__);
02067 Mat cornersForDisplay(corners);
02068 debugDisplayPattern(image, cvSize(patternSize.width, patternSize.height), cornersForDisplay, true, 50.0);
02069 }
02070
02071
02072
02073
02074 retVal = verifyCorners(image.size(), patternSize, corners, MIN_CORNER_SEPARATION, MAX_CORNER_SEPARATION);
02075
02076 if (!retVal)
02077 {
02078 printf("%s << Pattern verification failed.\n", __FUNCTION__);
02079 corners.clear();
02080 } else {
02081 printf("%s << Pattern verification succeeded.\n", __FUNCTION__);
02082 }
02083
02084 return retVal;
02085
02086 }
02087
02088 bool verifyCorners(Size imSize, Size patternSize, vector<Point2f>& patternPoints, double minDist, double maxDist)
02089 {
02090 vector<Point2f> simplePoints;
02091
02092 for (unsigned int i = 0; i < patternPoints.size(); i++)
02093 {
02094 simplePoints.push_back(Point(patternPoints.at(i).x, patternPoints.at(i).y));
02095 }
02096
02097 return verifyPattern(imSize, patternSize, simplePoints, minDist, maxDist);
02098 }
02099
02100 void refineCornerPositions(const Mat& image, Size patternSize, vector<Point2f>& vCorners)
02101 {
02102
02103 Mat imGrey;
02104
02105 vector<Point2f> targetCorner;
02106
02107 if (image.channels() > 1)
02108 {
02109 cvtColor(image, imGrey, CV_RGB2GRAY);
02110 }
02111 else
02112 {
02113 image.copyTo(imGrey);
02114 }
02115
02116 vector<Point2f> neighbouringPts;
02117
02118 int X, Y;
02119
02120 X = patternSize.width/2;
02121 Y = patternSize.height/2;
02122
02123 vector<Point2f> fullCornerGrid;
02124
02125 double minDimension = findMinimumSeparation(vCorners);
02126
02127 int correctionDistance;
02128
02129
02130
02131
02132 for (int i = 0; i < Y; i++)
02133 {
02134 for (int j = 0; j < X; j++)
02135 {
02136 for (int k = 0; k < 4; k++)
02137 {
02138 Point2f pt;
02139
02140 if (k == 0)
02141 {
02142 pt = Point2f(j-0.25, i-0.25);
02143 }
02144 else if (k == 1)
02145 {
02146 pt = Point2f(j+0.25, i-0.25);
02147 }
02148 else if (k == 2)
02149 {
02150 pt = Point2f(j-0.25, i+0.25);
02151 }
02152 else if (k == 3)
02153 {
02154 pt = Point2f(j+0.25, i+0.25);
02155 }
02156
02157 fullCornerGrid.push_back(pt);
02158 }
02159
02160 }
02161 }
02162
02163 vector<Point2f> patchNeighbourhood;
02164 vector<Point2f> patchArrangement;
02165 vector<Point2f> newCorners, bestCorners;
02166 vector<Point2f> cornerArrangement;
02167 Mat cornerLocs(2, 2, CV_32FC2);
02168
02169
02170
02171 Mat homography;
02172
02173
02174
02175 int neighbourhoodCount = 0;
02176
02177 int index = 0;
02178
02179 int groupedIndex = -1;
02180
02181 newCorners.assign(vCorners.begin(), vCorners.end());
02182
02183 if (DEBUG_MODE > 2)
02184 {
02185 vector<Point2f> redistorted;
02186 Mat cornersForDisplay(newCorners);
02187 printf("%s << Initial corners.\n", __FUNCTION__);
02188 debugDisplayPattern(image, cvSize(patternSize.width, patternSize.height), cornersForDisplay);
02189
02190 }
02191
02192 index = 0;
02193
02194
02195 for (int i = 0; i < 2*Y; i++)
02196 {
02197 for (int j = 0; j < 2*X; j++)
02198 {
02199
02200
02201 if (((i == 0) || (i == 2*Y-1) || (j == 0) || (j == 2*X-1)) && !(((i < 2) || (i > 2*Y-3)) && ((j < 2) || (j > 2*X-3))))
02202 {
02203
02204 patchNeighbourhood.clear();
02205 patchArrangement.clear();
02206
02207 neighbourhoodCount = 0;
02208
02209
02210 index = 2*X*i + j;
02211 groupedIndex = 4*(X*(i/2)+(j/2)) + (j%2) + 2*(i%2);
02212
02213
02214
02215 int z[4], c[4];
02216
02217 if (i == 0)
02218 {
02219 c[0] = index+2*X-1;
02220 c[1] = index+2*X+1;
02221 c[2] = index+4*X-1;
02222 c[3] = index+4*X+1;
02223 }
02224 else if (i == 2*Y-1)
02225 {
02226 c[0] = index-4*X-1;
02227 c[1] = index-4*X+1;
02228 c[2] = index-2*X-1;
02229 c[3] = index-2*X+1;
02230 }
02231 else if (j == 0)
02232 {
02233 c[0] = index-2*X+1;
02234 c[1] = index-2*X+2;
02235 c[2] = index+2*X+1;
02236 c[3] = index+2*X+2;
02237 }
02238 else if (j == 2*X-1)
02239 {
02240 c[0] = index-2*X-2;
02241 c[1] = index-2*X-1;
02242 c[2] = index+2*X-2;
02243 c[3] = index+2*X-1;
02244 }
02245
02246 int m, n;
02247
02248 for (int k = 0; k < 4; k++)
02249 {
02250
02251 m = c[k] / (2*X);
02252
02253 n = c[k] % (2*X);
02254
02255
02256
02257 z[k] = 4*(X*(m/2)+(n/2)) + (n%2) + 2*(m%2);
02258
02259
02260
02261 }
02262
02263
02264
02265 for (int k = 0; k < 4; k++)
02266 {
02267
02268 patchNeighbourhood.push_back(newCorners.at(z[k]));
02269 patchArrangement.push_back(fullCornerGrid.at(z[k]));
02270
02271
02272 neighbourhoodCount++;
02273 }
02274
02275
02276
02277
02278 homography = findHomography(Mat(patchArrangement), Mat(patchNeighbourhood));
02279
02280
02281 cornerArrangement.clear();
02282
02283 cornerArrangement.push_back(fullCornerGrid.at(groupedIndex));
02284
02285
02286 Mat tmpMat1 = Mat(cornerArrangement);
02287
02288 perspectiveTransform(tmpMat1, cornerLocs, homography);
02289
02290 newCorners.at(groupedIndex) = Point2f(cornerLocs.at<Vec3f>(0,0)[0], cornerLocs.at<Vec3f>(0,0)[1]);
02291
02292
02293 minDimension = findMinimumSeparation(patchNeighbourhood);
02294 correctionDistance = max(int(double(minDimension)/4.0), MINIMUM_CORRECTION_DISTANCE);
02295
02296
02297
02298
02299 targetCorner.clear();
02300 targetCorner.push_back(newCorners.at(groupedIndex));
02301 cornerSubPix(imGrey, targetCorner, Size(correctionDistance, correctionDistance), Size(-1,-1), cvTermCriteria(CV_TERMCRIT_EPS+CV_TERMCRIT_ITER, 15, 0.1));
02302 newCorners.at(groupedIndex) = targetCorner.at(0);
02303
02304 }
02305 }
02306 }
02307
02308
02309
02310
02311
02312
02313
02314
02315
02316
02317
02318
02319
02320 if (DEBUG_MODE > 2)
02321 {
02322 vector<Point2f> redistorted;
02323 Mat cornersForDisplay(newCorners);
02324 printf("%s << Refinement of edges.\n", __FUNCTION__);
02325 debugDisplayPattern(image, cvSize(patternSize.width, patternSize.height), cornersForDisplay, false);
02326
02327 }
02328
02329 index = 0;
02330
02331
02332 for (int i = 0; i < 2*Y; i++)
02333 {
02334 for (int j = 0; j < 2*X; j++)
02335 {
02336
02337
02338 if ((((i == 1) || (i == 2*Y-2)) && ((j == 0) || (j == 2*X-1))) || (((i == 0) || (i == 2*Y-1)) && ((j == 1) || (j == 2*X-2))))
02339 {
02340
02341 patchNeighbourhood.clear();
02342 patchArrangement.clear();
02343
02344 neighbourhoodCount = 0;
02345
02346
02347 index = 2*X*i + j;
02348 groupedIndex = 4*(X*(i/2)+(j/2)) + (j%2) + 2*(i%2);
02349
02350
02351
02352 int z[4], c[4];
02353
02354
02355
02356
02357 if ((i == 0) && (j == 1))
02358 {
02359 c[0] = index+1;
02360 c[1] = index+2*X;
02361 c[2] = index+2*X+2;
02362 c[3] = index+4*X+1;
02363 }
02364 else if ((i == 1) && (j == 0))
02365 {
02366 c[0] = index+1;
02367 c[1] = index+2*X;
02368 c[2] = index+2*X+2;
02369 c[3] = index+4*X+1;
02370 }
02371 else if ((i == 0) && (j == 2*X-2))
02372 {
02373 c[0] = index-1;
02374 c[1] = index+2*X-2;
02375 c[2] = index+2*X;
02376 c[3] = index+4*X-1;
02377 }
02378 else if ((i == 1) && (j == 2*X-1))
02379 {
02380 c[0] = index-1;
02381 c[1] = index+2*X-2;
02382 c[2] = index+2*X;
02383 c[3] = index+4*X-1;
02384 }
02385 else if ((i == 2*Y-2) && (j == 0))
02386 {
02387 c[0] = index-4*X+1;
02388 c[1] = index-2*X;
02389 c[2] = index-2*X+2;
02390 c[3] = index+1;
02391 }
02392 else if ((i == 2*Y-1) && (j == 1))
02393 {
02394 c[0] = index-4*X+1;
02395 c[1] = index-2*X;
02396 c[2] = index-2*X+2;
02397 c[3] = index+1;
02398 }
02399 else if ((i == 2*Y-2) && (j == 2*X-1))
02400 {
02401 c[0] = index-4*X-1;
02402 c[1] = index-2*X-2;
02403 c[2] = index-2*X;
02404 c[3] = index-1;
02405 }
02406 else if ((i == 2*Y-1) && (j == 2*X-2))
02407 {
02408 c[0] = index-4*X-1;
02409 c[1] = index-2*X-2;
02410 c[2] = index-2*X;
02411 c[3] = index-1;
02412 }
02413
02414
02415 int m, n;
02416
02417 for (int k = 0; k < 4; k++)
02418 {
02419
02420 m = c[k] / (2*X);
02421
02422 n = c[k] % (2*X);
02423
02424
02425
02426 z[k] = 4*(X*(m/2)+(n/2)) + (n%2) + 2*(m%2);
02427
02428
02429
02430 }
02431
02432
02433
02434 for (int k = 0; k < 4; k++)
02435 {
02436
02437 patchNeighbourhood.push_back(newCorners.at(z[k]));
02438 patchArrangement.push_back(fullCornerGrid.at(z[k]));
02439
02440
02441 neighbourhoodCount++;
02442 }
02443
02444
02445
02446
02447 homography = findHomography(Mat(patchArrangement), Mat(patchNeighbourhood));
02448
02449
02450 cornerArrangement.clear();
02451
02452 cornerArrangement.push_back(fullCornerGrid.at(groupedIndex));
02453
02454
02455 Mat tmpMat1 = Mat(cornerArrangement);
02456
02457 perspectiveTransform(tmpMat1, cornerLocs, homography);
02458
02459 newCorners.at(groupedIndex) = Point2f(cornerLocs.at<Vec3f>(0,0)[0], cornerLocs.at<Vec3f>(0,0)[1]);
02460
02461
02462
02463 minDimension = findMinimumSeparation(patchNeighbourhood);
02464 correctionDistance = max(int(double(minDimension)/4.0), MINIMUM_CORRECTION_DISTANCE);
02465
02466
02467
02468
02469 targetCorner.clear();
02470 targetCorner.push_back(newCorners.at(groupedIndex));
02471 cornerSubPix(imGrey, targetCorner, Size(correctionDistance, correctionDistance), Size(-1,-1), cvTermCriteria(CV_TERMCRIT_EPS+CV_TERMCRIT_ITER, 15, 0.1));
02472 newCorners.at(groupedIndex) = targetCorner.at(0);
02473
02474 }
02475 }
02476 }
02477
02478
02479
02480
02481
02482
02483
02484
02485
02486
02487
02488
02489
02490
02491
02492 if (DEBUG_MODE > 2)
02493 {
02494 vector<Point2f> redistorted;
02495 Mat cornersForDisplay(newCorners);
02496 printf("%s << Refinement of psuedocorners.\n", __FUNCTION__);
02497 debugDisplayPattern(image, cvSize(patternSize.width, patternSize.height), cornersForDisplay, false);
02498 }
02499
02500 index = 0;
02501
02502
02503 for (int i = 0; i < 2*Y; i++)
02504 {
02505 for (int j = 0; j < 2*X; j++)
02506 {
02507
02508
02509 if (((i == 0) || (i == 2*Y-1)) && ((j == 0) || (j == 2*X-1)))
02510 {
02511
02512 patchNeighbourhood.clear();
02513 patchArrangement.clear();
02514
02515 neighbourhoodCount = 0;
02516
02517
02518 index = 2*X*i + j;
02519 groupedIndex = 4*(X*(i/2)+(j/2)) + (j%2) + 2*(i%2);
02520
02521
02522
02523 int z[4], c[4];
02524
02525
02526
02527
02528 if ((i == 0) && (j == 0))
02529 {
02530 c[0] = index+1;
02531 c[1] = index+2*X;
02532 c[2] = index+2*X+2;
02533 c[3] = index+4*X+1;
02534 }
02535 else if ((i == 0) && (j == 2*X-1))
02536 {
02537 c[0] = index-1;
02538 c[1] =index+2*X-2;
02539 c[2] = index+2*X;
02540 c[3] = index+4*X-1;
02541 }
02542 else if ((i == 2*Y-1) && (j == 0))
02543 {
02544 c[0] = index-4*X+1;
02545 c[1] = index-2*X;
02546 c[2] = index-2*X+2;
02547 c[3] = index+1;
02548 }
02549 else if ((i == 2*Y-1) && (j == 2*X-1))
02550 {
02551 c[0] = index-4*X-1;
02552 c[1] = index-2*X-2;
02553 c[2] = index-2*X;
02554 c[3] = index-1;
02555 }
02556
02557
02558 int m, n;
02559
02560 for (int k = 0; k < 4; k++)
02561 {
02562
02563 m = c[k] / (2*X);
02564
02565 n = c[k] % (2*X);
02566
02567
02568
02569 z[k] = 4*(X*(m/2)+(n/2)) + (n%2) + 2*(m%2);
02570
02571
02572
02573 }
02574
02575
02576
02577 for (int k = 0; k < 4; k++)
02578 {
02579
02580 patchNeighbourhood.push_back(newCorners.at(z[k]));
02581 patchArrangement.push_back(fullCornerGrid.at(z[k]));
02582
02583
02584 neighbourhoodCount++;
02585 }
02586
02587
02588
02589
02590 homography = findHomography(Mat(patchArrangement), Mat(patchNeighbourhood));
02591
02592
02593 cornerArrangement.clear();
02594
02595 cornerArrangement.push_back(fullCornerGrid.at(groupedIndex));
02596
02597
02598 Mat tmpMat1 = Mat(cornerArrangement);
02599
02600 perspectiveTransform(tmpMat1, cornerLocs, homography);
02601
02602 newCorners.at(groupedIndex) = Point2f(cornerLocs.at<Vec3f>(0,0)[0], cornerLocs.at<Vec3f>(0,0)[1]);
02603
02604
02605 minDimension = findMinimumSeparation(patchNeighbourhood);
02606 correctionDistance = max(int(double(minDimension)/4.0), 5);
02607
02608
02609
02610
02611 targetCorner.clear();
02612 targetCorner.push_back(newCorners.at(groupedIndex));
02613 cornerSubPix(imGrey, targetCorner, Size(correctionDistance, correctionDistance), Size(-1,-1), cvTermCriteria(CV_TERMCRIT_EPS+CV_TERMCRIT_ITER, 15, 0.1));
02614 newCorners.at(groupedIndex) = targetCorner.at(0);
02615
02616 }
02617 }
02618 }
02619
02620
02621
02622
02623
02624
02625
02626
02627
02628
02629
02630
02631
02632 if (DEBUG_MODE > 2)
02633 {
02634 vector<Point2f> redistorted;
02635 Mat cornersForDisplay(newCorners);
02636 printf("%s << Refinement of true corners.\n", __FUNCTION__);
02637 debugDisplayPattern(image, cvSize(patternSize.width, patternSize.height), cornersForDisplay, false);
02638
02639 }
02640
02641 index = 0;
02642
02643 vCorners.clear();
02644 vCorners.assign(newCorners.begin(), newCorners.end());
02645
02646 }
02647
02648 void groupPointsInQuads(Size patternSize, vector<Point2f>& corners)
02649 {
02650 vector<Point2f> newCorners;
02651
02652 int id1, id2, id3, id4;
02653
02654 for (int i = 0; i < patternSize.height/2; i++)
02655 {
02656 for (int j = 0; j < patternSize.width/2; j++)
02657 {
02658
02659 id1 = 2*(i*patternSize.width+j);
02660 id2 = 2*(i*patternSize.width+j)+1;
02661 id3 = 2*(i*patternSize.width+j)+patternSize.width;
02662 id4 = 2*(i*patternSize.width+j)+patternSize.width+1;
02663
02664
02665 newCorners.push_back(corners.at(id1));
02666 newCorners.push_back(corners.at(id2));
02667 newCorners.push_back(corners.at(id3));
02668 newCorners.push_back(corners.at(id4));
02669 }
02670 }
02671
02672 corners.assign(newCorners.begin(), newCorners.end());
02673 }
02674
02675 int findBestCorners(const Mat& image, vector<Point2f>& src, vector<Point2f>& dst, Size patternSize, int detector, int searchDist)
02676 {
02677
02678 int successfulMappings = 0;
02679
02680 double maxDist = 0.0;
02681
02682 vector<Point2f> foundCorners;
02683 int maxCorners = 4*src.size();
02684
02685 unsigned int *srcBestMatches;
02686 unsigned int foundBestMatch;
02687 double bestDist, currDist;
02688
02689
02690
02691
02692
02693
02694
02695
02696
02697 vector<KeyPoint> keypoints;
02698
02699
02700
02701
02702 Mat imGrey, tmpMat;
02703 if (image.channels() > 1)
02704 {
02705 cvtColor(image, imGrey, CV_RGB2GRAY);
02706 }
02707 else
02708 {
02709 image.copyTo(imGrey);
02710 }
02711
02712
02713 Mat imageCopy(480, 640, CV_8UC1);
02714 Mat inputDisp;
02715
02716 #ifdef _WIN32
02717
02718
02719
02720
02721
02722 #endif
02723
02724
02725
02726 switch (detector)
02727 {
02728
02729 case 0:
02730
02731
02732 dst.assign(src.begin(), src.end());
02733 return 0;
02734
02735
02736 case 1:
02737
02738
02739
02740
02741
02742
02743
02744 goodFeaturesToTrack(imGrey, foundCorners, maxCorners, 0.001, 5.0, Mat(), 11, true, 0.04);
02745
02746
02747
02748 break;
02749
02750 case 2:
02751
02752 #ifdef _WIN32
02753
02754
02755
02756
02757
02758
02759
02760
02761
02762
02763
02764
02765
02766
02767
02768
02769
02770
02771
02772
02773
02774
02775
02776
02777
02778
02779
02780
02781 break;
02782
02783 #endif
02784
02785 #ifndef _WIN32
02786
02787 dst.assign(src.begin(), src.end());
02788 return 0;
02789 #endif
02790
02791 case 3:
02792
02793
02794
02795
02796 FAST(imGrey, keypoints, 16);
02797
02798 for (unsigned int i = 0; i < keypoints.size(); i++)
02799 {
02800 foundCorners.push_back(keypoints.at(i).pt);
02801 }
02802
02803
02804
02805 break;
02806
02807 case 4:
02808
02809
02810
02811
02812
02813
02814
02815
02816 initialRefinementOfCorners(imGrey, src, patternSize);
02817
02818 dst.assign(src.begin(), src.end());
02819
02820 if (DEBUG_MODE > 3)
02821 {
02822 image.copyTo(inputDisp);
02823 drawChessboardCorners(inputDisp, cvSize(12, 8), Mat(dst), true);
02824 imshow("mainWin", inputDisp);
02825 waitKey(0);
02826 }
02827
02828 return 0;
02829
02830
02831 case 5:
02832
02833 cornerSubPix(image, src, Size(searchDist*2, searchDist*2), Size(-1,-1), cvTermCriteria(CV_TERMCRIT_EPS+CV_TERMCRIT_ITER, 15, 0.1));
02834
02835 maxDist = 0.5;
02836
02837 #ifdef _WIN32
02838
02839
02840
02841
02842
02843
02844
02845
02846
02847
02848
02849
02850
02851
02852
02853
02854
02855
02856
02857
02858
02859
02860
02861
02862
02863
02864
02865
02866 break;
02867
02868 #endif
02869
02870 #ifndef _WIN32
02871
02872 dst.assign(src.begin(), src.end());
02873 return 0;
02874 #endif
02875 default:
02876 return -1;
02877 }
02878
02879
02880
02881
02882
02883 Mat imCpy(imGrey.rows, imGrey.cols, CV_8UC3);
02884 Mat imCpy2;
02885
02886 cvtColor(imGrey, imCpy, CV_GRAY2RGB);
02887
02888 resize(imCpy, imCpy2, Size(), 2.0, 2.0);
02889
02890 Scalar color;
02891
02892 for (unsigned int i = 0; i < foundCorners.size(); i++)
02893 {
02894 color = Scalar( rand()&255, rand()&255, rand()&255 );
02895 circle(imCpy2, Point2f(foundCorners.at(i).x * 2.0, foundCorners.at(i).y * 2.0), 4, color, 2);
02896 }
02897
02898 imshow("rawCorners", imCpy2);
02899 waitKey(20);
02900
02901 srcBestMatches = new unsigned int[src.size()];
02902
02903
02904 for (unsigned int i = 0; i < src.size(); i++)
02905 {
02906 bestDist = 9e99;
02907 for (unsigned int j = 0; j < foundCorners.size(); j++)
02908 {
02909 currDist = pow(pow(double(src.at(i).x) - double(foundCorners.at(j).x), 2.0) + pow(double(src.at(i).y) - double(foundCorners.at(j).y), 2.0), 0.5);
02910
02911 if (currDist < bestDist)
02912 {
02913 bestDist = currDist;
02914 srcBestMatches[i] = j;
02915 }
02916 }
02917 }
02918
02919
02920 for (unsigned int i = 0; i < src.size(); i++)
02921 {
02922 bestDist = 9e99;
02923 for (unsigned int k = 0; k < src.size(); k++)
02924 {
02925
02926 currDist = pow(pow(double(src.at(k).x) - double(foundCorners.at(srcBestMatches[i]).x), 2.0) + pow(double(src.at(k).y) - double(foundCorners.at(srcBestMatches[i]).y), 2.0), 0.5);
02927
02928 if (currDist < bestDist)
02929 {
02930 bestDist = currDist;
02931 foundBestMatch = k;
02932 }
02933 }
02934
02935
02936 if ((foundBestMatch == i) && (bestDist < searchDist))
02937 {
02938 successfulMappings++;
02939 dst.push_back(foundCorners.at(srcBestMatches[i]));
02940 }
02941 else
02942 {
02943 dst.push_back(src.at(i));
02944 }
02945 }
02946
02947
02948
02949
02950
02951
02952
02953
02954
02955
02956
02957 delete[] srcBestMatches;
02958
02959
02960
02961 return successfulMappings;
02962 }
02963
02964 void initialRefinementOfCorners(const Mat& imGrey, vector<Point2f>& src, Size patternSize)
02965 {
02966
02967
02968 int correctionDistance;
02969 double minDimension;
02970
02971 Mat imDisplay;
02972 imGrey.copyTo(imDisplay);
02973
02974 Mat forDisplay(src);
02975
02976 Mat forDisp2;
02977
02978 if (DEBUG_MODE > 2)
02979 {
02980 debugDisplayPattern(imDisplay, patternSize, forDisplay, false);
02981 }
02982
02983
02984 vector<Point2f> ptNeighbourhood, targetCorner, dst;
02985
02986 dst.assign(src.begin(), src.end());
02987
02988
02989
02990 for (int j = 1; j < patternSize.height-1; j++)
02991 {
02992 for (int i = 1; i < patternSize.width-1; i++)
02993 {
02994
02995 ptNeighbourhood.clear();
02996
02997
02998
02999
03000 int val;
03001
03002 val = (j-1)*patternSize.width+(i-1);
03003 ptNeighbourhood.push_back(src.at(val));
03004
03005
03006 val = (j-1)*patternSize.width+(i+1);
03007 ptNeighbourhood.push_back(src.at(val));
03008
03009
03010 val = (j+1)*patternSize.width+(i-1);
03011 ptNeighbourhood.push_back(src.at(val));
03012
03013
03014 val = (j+1)*patternSize.width+(i+1);
03015 ptNeighbourhood.push_back(src.at(val));
03016
03017
03018 imGrey.copyTo(imDisplay);
03019
03020 forDisp2 = Mat(ptNeighbourhood);
03021
03022 if (DEBUG_MODE > 2)
03023 {
03024
03025 }
03026
03027
03028 minDimension = findMinimumSeparation(ptNeighbourhood);
03029
03030
03031
03032 correctionDistance = max(int(double(minDimension)/4), 1);
03033
03034
03035
03036 targetCorner.clear();
03037 targetCorner.push_back(src.at(j*patternSize.width+i));
03038
03039 cornerSubPix(imGrey, targetCorner, Size(correctionDistance, correctionDistance), Size(-1,-1), cvTermCriteria(CV_TERMCRIT_EPS+CV_TERMCRIT_ITER, 15, 0.1));
03040
03041 dst.at(j*patternSize.width+i) = targetCorner.at(0);
03042
03043 }
03044 }
03045
03046 Mat imCol;
03047
03048 cvtColor(imDisplay, imCol, CV_GRAY2RGB);
03049
03050
03051
03052
03053
03054
03055
03056
03057
03058
03059
03060
03061
03062
03063
03064 src.assign(dst.begin(), dst.end());
03065
03066
03067
03068
03069
03070
03071 }
03072
03073 void sortCorners(Size imageSize, Size patternSize, vector<Point2f>& corners)
03074 {
03075 vector<Point2f> newCorners;
03076
03077 int X = patternSize.width/2;
03078 int Y = patternSize.height/2;
03079
03080
03081
03082
03083 for (int i = 0; i < Y; i++)
03084 {
03085 for (int j = 0; j < X; j++)
03086 {
03087 if (DEBUG_MODE > 3)
03088 {
03089
03090 }
03091 newCorners.push_back(corners.at(4*i*X+j*4));
03092 newCorners.push_back(corners.at(4*i*X+j*4+1));
03093 }
03094
03095 for (int j = 0; j < X; j++)
03096 {
03097 if (DEBUG_MODE > 3)
03098 {
03099 printf("%d, %d, ", 4*i*X+j*4+2, 4*i*X+j*4+3);
03100 }
03101 newCorners.push_back(corners.at(4*i*X+j*4+2));
03102 newCorners.push_back(corners.at(4*i*X+j*4+3));
03103 }
03104
03105 }
03106
03107 corners.clear();
03108
03109 for (unsigned int i = 0; i < newCorners.size(); i++)
03110 {
03111 corners.push_back(newCorners.at(i));
03112 }
03113 }
03114
03115 bool correctPatchCentres(const Mat& image, Size patternSize, vector<Point2f>& patchCentres, int mode)
03116 {
03117
03118 bool found = true;
03119
03120 return found;
03121
03122 printf("%s << Entering...\n", __FUNCTION__);
03123
03124 Mat imGrey;
03125 if (image.channels() > 1)
03126 {
03127 cvtColor(image, imGrey, CV_RGB2GRAY);
03128 }
03129 else
03130 {
03131 image.copyTo(imGrey);
03132 }
03133
03134 Point3f newPoint;
03135 cv::vector<Point3f> row;
03136
03137
03138
03139 if (mode == 1)
03140 {
03141 for (int i = 0; i < patternSize.height/2; i++)
03142 {
03143 for (int j = 0; j < patternSize.width/2; j++)
03144 {
03145 newPoint = Point3f(float(i), float(j), 0.0);
03146 row.push_back(newPoint);
03147 }
03148
03149 }
03150 }
03151 else
03152 {
03153 for (int i = 0; i < patternSize.height+1; i++)
03154 {
03155 for (int j = 0; j < patternSize.width+1; j++)
03156 {
03157 newPoint = Point3f(float(i), float(j), 0.0);
03158 row.push_back(newPoint);
03159 }
03160
03161 }
03162 }
03163
03164 if (DEBUG_MODE > 2)
03165 {
03166 Mat cornersMat(patchCentres);
03167 debugDisplayPattern(image, cvSize(patternSize.width/2, patternSize.height/2), cornersMat);
03168 }
03169
03170
03171
03172
03173
03174 cv::vector< cv::vector<Point3f> > objectPoints;
03175 objectPoints.push_back(row);
03176
03177 vector<vector<Point2f> > vvOriginalCentres;
03178 vvOriginalCentres.push_back(patchCentres);
03179
03180
03181
03182 Mat cameraMatrix, distCoeffs, newCamMat;
03183
03184 cv::vector<Mat> rvecs, tvecs;
03185
03186 calibrateCamera(objectPoints, vvOriginalCentres, image.size(), cameraMatrix, distCoeffs, rvecs, tvecs, PATCH_CORRECTION_INTRINSICS_FLAGS);
03187
03188 double alpha = 0.5;
03189
03190 Rect validROI;
03191 newCamMat = getOptimalNewCameraMatrix(cameraMatrix, distCoeffs, image.size(), alpha, image.size(), &validROI);
03192
03193 Mat undistortedImage;
03194 undistort(image, undistortedImage, cameraMatrix, distCoeffs, newCamMat);
03195
03196 vector<Point2f> newCentres;
03197 found = findPatternCentres(undistortedImage, patternSize, newCentres, mode);
03198
03199 if (!found)
03200 {
03201 printf("%s << Pattern could not be found once image was undistorted..\n", __FUNCTION__);
03202
03203 return found;
03204 }
03205
03206 printf("%s << Redistorting MSER centroids...\n", __FUNCTION__);
03207
03208 redistortPoints(newCentres, patchCentres, cameraMatrix, distCoeffs, newCamMat);
03209 printf("%s << newCentres.size() = %d\n", __FUNCTION__, newCentres.size());
03210 printf("%s << patchCentres.size() = %d\n", __FUNCTION__, patchCentres.size());
03211 printf("%s << DONE.\n", __FUNCTION__);
03212
03213 if (DEBUG_MODE > 2)
03214 {
03215 Mat cornersMat(patchCentres);
03216 debugDisplayPattern(image, cvSize(patternSize.width/2, patternSize.height/2), cornersMat);
03217 }
03218
03219
03220
03221
03222
03223
03224
03225
03226
03227
03228
03229
03230
03231
03232
03233
03234
03235
03236
03237
03238
03239
03240
03241
03242
03243
03244
03245
03246
03247
03248
03249
03250
03251
03252
03253
03254
03255
03256
03257
03258
03259
03260
03261
03262
03263
03264
03265
03266
03267
03268
03269
03270 return found;
03271 }
03272
03273 bool findPatternCentres(const Mat& image, Size patternSize, vector<Point2f>& centres, int mode)
03274 {
03275
03276
03277
03278 if (!checkAcutance())
03279 {
03280 return false;
03281 }
03282
03283 int patchCols, patchRows, desiredPatchQuantity;
03284 determinePatchDistribution(patternSize, mode, patchRows, patchCols, desiredPatchQuantity);
03285
03286 vector<vector<Point> > msers;
03287 findAllPatches(image, patternSize, msers);
03288
03289 if (DEBUG_MODE > 3)
03290 {
03291 debugDisplayPatches(image, msers);
03292 }
03293
03294 if (int(msers.size()) < desiredPatchQuantity)
03295 {
03296 centres.clear();
03297 if (DEBUG_MODE > 1)
03298 {
03299 printf("%s << Insufficient patches found. Returning.\n", __FUNCTION__);
03300 }
03301
03302 return false;
03303 }
03304
03305 bool found = refinePatches(image, patternSize, msers, centres, mode);
03306
03307 if (DEBUG_MODE > 1)
03308 {
03309 printf("%s << Patches found after refinement = %d\n", __FUNCTION__, msers.size());
03310 }
03311
03312 if (DEBUG_MODE > 3)
03313 {
03314 debugDisplayPatches(image, msers);
03315 }
03316
03317
03318 if (!found)
03319 {
03320 centres.clear();
03321
03322 if (DEBUG_MODE > 1)
03323 {
03324 printf("%s << Correct number of patches not found. Returning.\n", __FUNCTION__);
03325 }
03326
03327 return false;
03328 }
03329
03330 sortPatches(image.size(), patternSize, centres, mode);
03331
03332 if (DEBUG_MODE > 3)
03333 {
03334 Mat patchCentres_(centres);
03335 debugDisplayPattern(image, cvSize(patchCols, patchRows), patchCentres_);
03336 }
03337
03338 found = verifyPatches(image.size(), patternSize, centres, mode, MIN_CORNER_SEPARATION, MAX_CORNER_SEPARATION);
03339
03340 if (!found)
03341 {
03342 centres.clear();
03343
03344 if (DEBUG_MODE > 1)
03345 {
03346 printf("%s << Pattern verification failed. Returning.\n", __FUNCTION__);
03347 }
03348
03349 return false;
03350 }
03351
03352 return found;
03353 }
03354
03355 bool verifyPattern(Size imSize, Size patternSize, vector<Point2f>& patternPoints, double minDist, double maxDist)
03356 {
03357
03358
03359 bool retVal = true;
03360
03361 retVal = patternInFrame(imSize, patternPoints);
03362
03363 if (retVal == false)
03364 {
03365
03366 if (DEBUG_MODE > 1)
03367 {
03368 printf("%s << Some pattern points are outside valid area.\n", __FUNCTION__);
03369 }
03370
03371 return retVal;
03372 }
03373
03374 double dist, dist1, dist2, factor;
03375
03376 double maxStraightnessDist = 30.0;
03377
03378 int index;
03379
03380 Mat blackImage = Mat::zeros(imSize, CV_8UC3);
03381 Scalar blue(255, 0, 0), green(0, 255, 0), red(0, 0, 255);
03382
03383
03384
03385
03386 for (int i = 0; i < patternSize.height; i++)
03387 {
03388 for (int j = 0; j < patternSize.width-2; j++)
03389 {
03390
03391
03392 index = i*patternSize.width + j;
03393
03394 blackImage.setTo(0);
03395
03396
03397
03398
03399
03400
03401
03402
03403
03404
03405
03406
03407 dist = perpDist(patternPoints.at(index), patternPoints.at(index + 1), patternPoints.at(index + 2));
03408 dist1 = distBetweenPts2f(patternPoints.at(index),patternPoints.at(index + 1));
03409 dist2 = distBetweenPts2f(patternPoints.at(index + 1),patternPoints.at(index + 2));
03410
03411 factor = max(dist1, dist2) / min(dist1, dist2);
03412
03413
03414 if (dist > maxStraightnessDist)
03415 {
03416 if (DEBUG_MODE > 0)
03417 {
03418 printf("%s << Row out of alignment.\n", __FUNCTION__);
03419 }
03420
03421
03422 return false;
03423 }
03424
03425 if (factor > 2.0)
03426 {
03427 if (DEBUG_MODE > 0)
03428 {
03429 printf("%s << Row factor is out.\n", __FUNCTION__);
03430 printf("%s << factor / [dist1, dist2] = %f / [%f, %f].\n", __FUNCTION__, factor, dist1, dist2);
03431 }
03432
03433 return false;
03434 }
03435 }
03436 }
03437
03438
03439
03440
03441 for (int i = 0; i < patternSize.height-2; i++)
03442 {
03443 for (int j = 0; j < patternSize.width; j++)
03444 {
03445
03446 dist = perpDist(patternPoints.at(i*patternSize.width + j), patternPoints.at((i+1)*patternSize.width + j), patternPoints.at((i+2)*patternSize.width + j));
03447 dist1 = distBetweenPts2f(patternPoints.at(i*patternSize.width + j), patternPoints.at((i+1)*patternSize.width + j));
03448 dist2 = distBetweenPts2f(patternPoints.at((i+1)*patternSize.width + j), patternPoints.at((i+2)*patternSize.width + j));
03449
03450 factor = max(dist1, dist2) / min(dist1, dist2);
03451
03452 if (dist > maxStraightnessDist)
03453 {
03454 if (DEBUG_MODE > 0)
03455 {
03456 printf("%s << Column out of alignment.\n", __FUNCTION__);
03457 }
03458
03459 return false;
03460 }
03461
03462 if (factor > 2.0)
03463 {
03464 if (DEBUG_MODE > 0)
03465 {
03466 printf("%s << Column out of scale. 1\n", __FUNCTION__);
03467 }
03468
03469 return false;
03470 }
03471 }
03472 }
03473
03474
03475
03476 for (int i = 0; i < patternSize.height; i++) {
03477 for (int j = 0; j < patternSize.width; j++) {
03478
03479
03480
03481
03482 for (int a = 0; a < patternSize.height; a++) {
03483 for (int b = 0; b < patternSize.width; b++) {
03484
03485
03486
03487
03488 if ((a == i) && (b == j)) { continue; }
03489
03490 dist = distBetweenPts2f(patternPoints.at(i*patternSize.width + j), patternPoints.at(a*patternSize.width + b));
03491
03492 if ((dist > maxDist) && (a >= i-1) && (a <= i+1) && (b >= j-1) && (b <= j+1)) {
03493 if (DEBUG_MODE > 0) {
03494 printf("%s << Point (%d, %d) failed adjacent test (%f) [%d, %d] vs [%f, %f].\n", __FUNCTION__, i, j, dist, a, b, minDist, maxDist);
03495 }
03496
03497 return false;
03498 }
03499
03500 if (dist <= minDist) {
03501
03502 if (DEBUG_MODE > 0) {
03503 printf("%s << Point (%d, %d) failed closeness test (%f) [%d, %d] vs [%f, %f].\n", __FUNCTION__, i, j, dist, a, b, minDist, maxDist);
03504 }
03505
03506 return false;
03507
03508 }
03509
03510 if ((DEBUG_MODE > 0) && (dist < 1.0)) {
03511 printf("%s << Point (%d, %d) has distance of (%f) to [%d, %d] vs [%f, %f].\n", __FUNCTION__, i, j, dist, a, b, minDist, maxDist);
03512 }
03513 }
03514
03515
03516
03517
03518
03519 }
03520 }
03521 }
03522
03523
03524 return true;
03525 }
03526
03527 bool verifyPatches(Size imSize, Size patternSize, vector<Point2f>& patchCentres, int mode, double minDist, double maxDist)
03528 {
03529
03530 Size newPatternSize;
03531
03532 if (mode == 0)
03533 {
03534 newPatternSize = Size(patternSize.width+1, patternSize.height+1);
03535 }
03536 else
03537 {
03538 newPatternSize = Size(patternSize.width/2, patternSize.height/2);
03539 }
03540
03541 return verifyPattern(imSize, newPatternSize, patchCentres, minDist, maxDist);
03542
03543
03544 int64 t = getTickCount();
03545
03546
03547 for (unsigned int i = 0; i < 10; i++)
03548 {
03549 if (0)
03550 {
03551 if (DEBUG_MODE > 0)
03552 {
03553 t = getTickCount() - t;
03554 printf("%s << Algorithm duration: %fms\n", __FUNCTION__, t*1000/getTickFrequency());
03555 }
03556 return false;
03557 }
03558 }
03559
03560 if (DEBUG_MODE > 0)
03561 {
03562 t = getTickCount() - t;
03563 printf("%s << Algorithm duration: %fms\n", __FUNCTION__, t*1000/getTickFrequency());
03564 }
03565
03566 return true;
03567 }
03568
03569 bool patternInFrame(Size imSize, vector<Point2f>& patternPoints, int minBorder)
03570 {
03571
03572
03573 for (unsigned int i = 0; i < patternPoints.size(); i++)
03574 {
03575 if ((patternPoints.at(i).x >= imSize.width-minBorder) || (patternPoints.at(i).x < minBorder) || (patternPoints.at(i).y >= imSize.height-minBorder) || (patternPoints.at(i).y < minBorder))
03576 {
03577
03578 patternPoints.clear();
03579
03580 return false;
03581 }
03582 }
03583
03584
03585 return true;
03586
03587 }
03588
03589 void shapeFilter(vector<mserPatch>& patches, vector<vector<Point> >& msers)
03590 {
03591
03592
03593
03594
03595
03596
03597 vector<mserPatch> newPatches;
03598 vector<vector<Point> > newMsers;
03599
03600
03601 double cWidth, cHeight;
03602 double acceptableFactor = 2.0;
03603
03604 for (unsigned int i = 0; i < patches.size(); i++)
03605 {
03606 cWidth = 0;
03607 cHeight = 0;
03608
03609 contourDimensions(msers.at(i), cWidth, cHeight);
03610
03611
03612 if (((cHeight/cWidth) < acceptableFactor) && ((cWidth/cHeight) < acceptableFactor))
03613 {
03614 newMsers.push_back(msers.at(i));
03615 newPatches.push_back(patches.at(i));
03616 }
03617 }
03618
03619 patches.clear();
03620 msers.clear();
03621
03622 for (unsigned int i = 0; i < newPatches.size(); i++)
03623 {
03624 patches.push_back(newPatches.at(i));
03625 msers.push_back(newMsers.at(i));
03626 }
03627
03628 }
03629
03630 void varianceFilter(vector<mserPatch>& patches, vector<vector<Point> >& msers)
03631 {
03632 vector<mserPatch> newPatches;
03633 vector<vector<Point> > newMsers;
03634
03635
03636 double maxAcceptableVariance = 1024.0;
03637
03638 for (unsigned int i = 0; i < patches.size(); i++)
03639 {
03640 if (patches.at(i).varIntensity < maxAcceptableVariance)
03641 {
03642 newMsers.push_back(msers.at(i));
03643 newPatches.push_back(patches.at(i));
03644 }
03645 }
03646
03647 patches.clear();
03648 msers.clear();
03649
03650 for (unsigned int i = 0; i < newPatches.size(); i++)
03651 {
03652 patches.push_back(newPatches.at(i));
03653 msers.push_back(newMsers.at(i));
03654 }
03655
03656 }
03657
03658 void enclosureFilter(vector<mserPatch>& patches, vector<vector<Point> >& msers)
03659 {
03660
03661 unsigned int i = 0, j = 0;
03662
03663 bool encloses = false;
03664
03665 double enclosureScore = 0.0;
03666
03667 while (i < patches.size())
03668 {
03669 encloses = false;
03670 j = 0;
03671
03672
03673
03674 while ((j < patches.size()) && !encloses)
03675 {
03676
03677
03678
03679 if (j != i)
03680 {
03681 if (patches.at(i).area >= patches.at(j).area)
03682 {
03683
03684
03685
03686
03687
03688
03689 enclosureScore = pointPolygonTest(Mat(patches.at(i).hull), patches.at(j).centroid, false);
03690
03691
03692
03693 if (enclosureScore > 0.0)
03694 {
03695 encloses = true;
03696
03697 }
03698 }
03699 }
03700
03701 j++;
03702 }
03703
03704
03705
03706 if (encloses)
03707 {
03708 patches.erase(patches.begin() + i);
03709 msers.erase(msers.begin() + i);
03710 }
03711 else
03712 {
03713 i++;
03714 }
03715 }
03716
03717
03718
03719 return;
03720
03721
03722
03723
03724 vector<mserPatch> newPatches;
03725 vector<vector<Point> > newMsers;
03726 double enclosed = -1.0;
03727 Point avCenter(0, 0);
03728 unsigned int accumulatedCount = 0;
03729
03730 for (unsigned int i = 0; i < patches.size()-1; i++)
03731 {
03732
03733 enclosed = -1.0;
03734
03735
03736 if (i == 0)
03737 {
03738 newPatches.push_back(patches.at(i));
03739 newMsers.push_back(msers.at(i));
03740
03741 avCenter += patches.at(i).centroid;
03742 accumulatedCount++;
03743 }
03744
03745
03746
03747 if (patches.at(i).area < patches.at(i+1).area)
03748 {
03749
03750 enclosed = pointPolygonTest(Mat(patches.at(i+1).hull), patches.at(i).centroid, false);
03751 }
03752
03753
03754
03755 if (enclosed < 0.0)
03756 {
03757
03758
03759
03760
03761
03762
03763
03764
03765
03766
03767
03768 accumulatedCount = 0;
03769 avCenter = Point(0, 0);
03770
03771 newPatches.push_back(patches.at(i+1));
03772 newMsers.push_back(msers.at(i+1));
03773 }
03774
03775 avCenter += patches.at(i+1).centroid;
03776 accumulatedCount++;
03777
03778 }
03779
03780 patches.clear();
03781 msers.clear();
03782
03783 for (unsigned int i = 0; i < newPatches.size(); i++)
03784 {
03785 patches.push_back(newPatches.at(i));
03786 msers.push_back(newMsers.at(i));
03787 }
03788
03789 }
03790
03791 void reduceCluster(vector<mserPatch>& patches, vector<vector<Point> >& msers, int totalPatches)
03792 {
03793
03794
03795
03796
03797
03798
03799
03800
03801
03802
03803
03804
03805
03806
03807
03808
03809
03810
03811
03812
03813
03814 vector<Point> pointsForArea;
03815 RotatedRect wrapperRectangle;
03816 Point2f circleCenter;
03817 float radius;
03818
03819 double totalArea;
03820 double newArea;
03821 double maxDiff = 0;
03822 int maxIndex = 0;
03823
03824
03825 while (patches.size() > ((unsigned int)totalPatches))
03826 {
03827 maxDiff = 0;
03828 maxIndex = 0;
03829
03830
03831 pointsForArea.clear();
03832 for (unsigned int i = 0; i < patches.size(); i++)
03833 {
03834 pointsForArea.push_back(patches.at(i).centroid);
03835 }
03836
03837 wrapperRectangle = fitEllipse(Mat(pointsForArea));
03838 minEnclosingCircle(Mat(pointsForArea), circleCenter, radius);
03839
03840
03841
03842 totalArea = radius;
03843
03844
03845
03846 for (unsigned int i = 0; i < patches.size(); i++)
03847 {
03848
03849
03850 pointsForArea.clear();
03851 for (unsigned int j = 0; j < patches.size(); j++)
03852 {
03853 if (j != i)
03854 {
03855
03856 pointsForArea.push_back(patches.at(j).centroid);
03857 }
03858 }
03859
03860 wrapperRectangle = fitEllipse(Mat(pointsForArea));
03861 minEnclosingCircle(Mat(pointsForArea), circleCenter, radius);
03862
03863
03864
03865 newArea = radius;
03866
03867
03868
03869
03870 if ((totalArea - newArea) > maxDiff)
03871 {
03872 maxDiff = totalArea - newArea;
03873 maxIndex = i;
03874
03875
03876 }
03877
03878 }
03879
03880
03881
03882 patches.erase(patches.begin() + maxIndex);
03883 msers.erase(msers.begin() + maxIndex);
03884
03885 }
03886
03887 return;
03888
03889 }
03890
03891 void clusterFilter(vector<mserPatch>& patches, vector<vector<Point> >& msers, int totalPatches)
03892 {
03893
03894
03895
03896
03897
03898
03899 vector<double> patchAreas, patchAreasSorted;
03900
03901 for (unsigned int iii = 0; iii < patches.size(); iii++) {
03902
03903 patchAreas.push_back(patches.at(iii).area);
03904
03905 }
03906
03907 while (patches.size() > ((unsigned int)totalPatches)) {
03908
03909
03910 patchAreasSorted.clear();
03911 patchAreasSorted.insert(patchAreasSorted.end(), patchAreas.begin(), patchAreas.end());
03912 sort(patchAreasSorted.begin(), patchAreasSorted.end());
03913
03914
03915
03916 for (unsigned int iii = 0; iii < patchAreasSorted.size(); iii++) {
03917
03918 }
03919
03920 double medianVal = patchAreas.at(int(patchAreasSorted.size() / 2));
03921
03922 double mean, stdev;
03923
03924 calcParameters(patchAreasSorted, mean, stdev);
03925
03926
03927
03928 double maxDiff = -1.0;;
03929 int maxDiffIndex = -1;
03930
03931
03932 for (unsigned int iii = 0; iii < patchAreasSorted.size(); iii++) {
03933
03934 }
03935
03936 for (unsigned int iii = 0; iii < patchAreasSorted.size(); iii++) {
03937
03938 if ((iii > 0) && (iii < patchAreasSorted.size()-1)) {
03939
03940 }
03941
03942 double tempDiff;
03943
03944
03945
03946
03947
03948
03949
03950
03951
03952
03953
03954
03955 tempDiff = abs(patchAreasSorted.at(iii) - mean);
03956
03957 if (tempDiff > maxDiff) {
03958 maxDiff = tempDiff;
03959 maxDiffIndex = iii;
03960 }
03961
03962
03963
03964
03965
03966
03967
03968
03969
03970
03971
03972
03973 }
03974
03975
03976
03977 double badArea = patchAreasSorted.at(maxDiffIndex);
03978
03979
03980
03981 if (abs(badArea - mean)/stdev < 3.0) {
03982
03983 return;
03984 }
03985
03986 bool culpritFound = false;
03987 unsigned int iii = 0;
03988 while (!culpritFound) {
03989
03990 if (patchAreas.at(iii) == badArea) {
03991 culpritFound = true;
03992 } else {
03993 iii++;
03994 }
03995 }
03996
03997 patchAreasSorted.erase(patchAreasSorted.begin() + maxDiffIndex);
03998 patchAreas.erase(patchAreas.begin() + iii);
03999
04000 patches.erase(patches.begin() + iii);
04001 msers.erase(msers.begin() + iii);
04002
04003
04004 }
04005
04006
04007 return;
04008
04009
04010
04011
04012
04013
04014
04015 vector<mserPatch> newPatches;
04016 vector<vector<Point> > newMsers;
04017
04018 bool clusterFound = false;
04019 unsigned int ii = 0, jj = 0;
04020 double minDist, maxDist;
04021 double minArea, maxArea;
04022 Point centroidA, centroidB;
04023 double intensityA, intensityB;
04024 double areaA, areaB;
04025 double areaVar = 1.0;
04026 double intensityVar = 32.0;
04027 double distVar = 1.0;
04028
04029
04030 newPatches.push_back(patches.at(patches.size()-1));
04031 patches.pop_back();
04032 newMsers.push_back(msers.at(msers.size()-1));
04033 msers.pop_back();
04034
04035 while (!clusterFound)
04036 {
04037
04038
04039 while (jj < newPatches.size())
04040 {
04041
04042
04043 centroidA = newPatches.at(jj).centroid;
04044 areaA = newPatches.at(jj).area;
04045 intensityA = newPatches.at(jj).meanIntensity;
04046
04047
04048
04049
04050
04051 minArea = areaA/(1+areaVar);
04052 maxArea = areaA*(1+areaVar);
04053 minDist = (2*pow(areaA, 0.5))/(1+distVar);
04054 maxDist = (2*pow(areaA, 0.5))*(1+distVar);
04055
04056
04057
04058
04059
04060
04061
04062
04063
04064 while (ii < patches.size())
04065 {
04066
04067
04068
04069 centroidB = patches.at(ii).centroid;
04070 areaB = patches.at(ii).area;
04071 intensityB = patches.at(ii).meanIntensity;
04072
04073
04074
04075
04076
04077
04078
04079
04080
04081
04082
04083 if ((distBetweenPts(centroidA, centroidB) < maxDist) && (distBetweenPts(centroidA, centroidB) > minDist))
04084 {
04085
04086 if (abs(intensityA - intensityB) < intensityVar)
04087 {
04088
04089 if (max(areaA/areaB, areaB/areaA) < (1 + areaVar))
04090 {
04091 transferMserElement(newMsers, msers, ii);
04092 transferPatchElement(newPatches, patches, ii);
04093 }
04094 else
04095 {
04096 ii++;
04097 }
04098 }
04099 else
04100 {
04101 ii++;
04102 }
04103 }
04104 else
04105 {
04106 ii++;
04107 }
04108 }
04109
04110 jj++;
04111 ii = 0;
04112 }
04113
04114 if (DEBUG_MODE > 1)
04115 {
04116 printf("%s << Cluster Size = %d\n", __FUNCTION__, newMsers.size());
04117 }
04118
04119
04120 if (newPatches.size() < ((unsigned int)totalPatches))
04121 {
04122 if (patches.size() >= ((unsigned int)totalPatches))
04123 {
04124 newPatches.clear();
04125 newMsers.clear();
04126
04127 newPatches.push_back(patches.at(patches.size()-1));
04128 patches.pop_back();
04129
04130 newMsers.push_back(msers.at(msers.size()-1));
04131 msers.pop_back();
04132
04133 }
04134 else
04135 {
04136 if (DEBUG_MODE > 1)
04137 {
04138 printf("%s << No cluster is large enough.\n", __FUNCTION__);
04139
04140 }
04141 return;
04142 }
04143 }
04144 else
04145 {
04146 clusterFound = true;
04147 if (DEBUG_MODE > 1)
04148 {
04149 printf("%s << Cluster found. size = %d.\n", __FUNCTION__, newPatches.size());
04150
04151 }
04152 }
04153
04154 ii = 0;
04155 jj = 0;
04156 }
04157
04158 patches.clear();
04159 msers.clear();
04160
04161 for (unsigned int i = 0; i < newPatches.size(); i++)
04162 {
04163 patches.push_back(newPatches.at(i));
04164 msers.push_back(newMsers.at(i));
04165 }
04166
04167 }
04168
04169 void transferPatchElement(vector<mserPatch>& dst, vector<mserPatch>& src, int index)
04170 {
04171
04172 dst.push_back(src.at(index));
04173
04174
04175 src.at(index) = src.at(src.size()-1);
04176
04177
04178 src.pop_back();
04179 }
04180
04181 void transferMserElement(vector<vector<Point> >& dst, vector<vector<Point> >& src, int index)
04182 {
04183
04184 dst.push_back(src.at(index));
04185
04186
04187 src.at(index) = src.at(src.size()-1);
04188
04189
04190 src.pop_back();
04191 }
04192
04193 bool refinePatches(const Mat& image, Size patternSize, vector<vector<Point> >& msers, vector<Point2f>& patchCentres, int mode)
04194 {
04195
04196
04197
04198
04199
04200
04201
04202
04203
04204 Mat colorImage;
04205
04206 if (DEBUG_MODE > 3) {
04207 if (image.channels() == 1) {
04208 cvtColor(image, colorImage, CV_GRAY2RGB);
04209 } else {
04210 image.copyTo(colorImage);
04211 }
04212
04213 }
04214
04215 int64 t = getTickCount();
04216
04217 vector<mserPatch> patches;
04218 vector<vector<Point> > newMsers, newerMsers, tmpMsers;
04219
04220
04221 vector<Point> hull;
04222 vector<Point> hull1, hull2;
04223 vector<Point> centroids;
04224 vector<Point> centroids2f;
04225 Moments moments1, moments2;
04226
04227 Mat imCpy, dispMat;
04228 Scalar color(0, 0, 255);
04229
04230 int x = patternSize.width, y = patternSize.height;
04231 double Xdb = double(x)/2, Ydb = double(y)/2;
04232 int X = x/2, Y = y/2;
04233 int totalPatches;
04234
04235 if (mode == 0)
04236 {
04237
04238 totalPatches = int(floor(( 2*floor(Xdb)*floor(Ydb)+1+floor(Xdb)+floor(Ydb)+(ceil(Xdb)-floor(Xdb))*floor(Ydb)+(ceil(Ydb)-floor(Ydb))*floor(Xdb)+(ceil(Xdb)-floor(Xdb))*(ceil(Ydb)-floor(Ydb)) + 0.01)));
04239 }
04240 else if (mode == 1)
04241 {
04242 totalPatches = X*Y;
04243 }
04244
04245
04246 if (DEBUG_MODE > 1)
04247 {
04248 printf("%s << totalPatches = %d\n", __FUNCTION__, totalPatches);
04249 printf("%s << Patches found before refinement = %d\n", __FUNCTION__, msers.size());
04250 }
04251
04252
04253
04254 patches.clear();
04255 for (unsigned int i = 0; i < msers.size(); i++)
04256 {
04257 patches.push_back(mserPatch(msers.at(i), image));
04258 }
04259
04260 if (DEBUG_MODE > 3)
04261 {
04262 printf("%s << Patches found before filtering = %d\n", __FUNCTION__, msers.size());
04263
04264
04265 colorImage.copyTo(imCpy);
04266 drawContours(imCpy, msers, -1, color, 2);
04267 imshow("mainWin", imCpy);
04268 waitKey(0);
04269 }
04270
04271
04272
04273
04274
04275
04276
04277
04278
04279 t = getTickCount();
04280
04281 shapeFilter(patches, msers);
04282
04283 if (DEBUG_MODE > 0)
04284 {
04285 t = getTickCount() - t;
04286 printf("%s << Shape Filter duration: %fms\n", __FUNCTION__, t*1000/getTickFrequency());
04287 printf("%s << Patches found after shape filter = %d\n", __FUNCTION__, msers.size());
04288 }
04289
04290 if (DEBUG_MODE > 6)
04291 {
04292 colorImage.copyTo(imCpy);
04293 drawContours(imCpy, msers, -1, color, 2);
04294 imshow("mainWin", imCpy);
04295 waitKey(0);
04296 }
04297
04298
04299
04300
04301
04302
04303
04304
04305
04306
04307
04308
04309
04310
04311
04312
04313
04314
04315
04316
04317
04318
04319
04320
04321
04322
04323
04324
04325
04326
04327
04328
04329
04330
04331
04332
04333
04334
04335 t = getTickCount();
04336
04337 enclosureFilter(patches, msers);
04338
04339
04340 if (patches.size() < ((unsigned int)totalPatches))
04341 {
04342 if (DEBUG_MODE > 1)
04343 {
04344 printf("There are an insufficient (%d/%d) number of patches after enclosure filter.\n", msers.size(), totalPatches);
04345 }
04346 return false;
04347 }
04348
04349
04350 if (DEBUG_MODE > 1)
04351 {
04352 printf("%s << Patches found after enclosure filter = %d\n", __FUNCTION__, msers.size());
04353 }
04354
04355 if (DEBUG_MODE > 6)
04356 {
04357 colorImage.copyTo(imCpy);
04358 color = Scalar(0, 255, 0);
04359 drawContours(imCpy, msers, -1, color, 2);
04360 imshow("mainWin", imCpy);
04361 waitKey(0);
04362 }
04363
04364 if (DEBUG_MODE > 0)
04365 {
04366 t = getTickCount() - t;
04367 printf("%s << Enclosure Filter duration: %fms\n", __FUNCTION__, t*1000/getTickFrequency());
04368 }
04369
04370
04371
04372
04373
04374
04375
04376
04377 int previousPatches = 9999;
04378
04379
04380
04381 while ((msers.size() > ((unsigned int)totalPatches)) && (msers.size() < ((unsigned int)previousPatches)))
04382 {
04383
04384 if (DEBUG_MODE > 1)
04385 {
04386 printf("%s << Looping through filters again...\n", __FUNCTION__);
04387 }
04388
04389 previousPatches = msers.size();
04390
04391
04392 t = getTickCount();
04393 clusterFilter(patches, msers, totalPatches);
04394
04395 if (DEBUG_MODE > 1)
04396 {
04397 printf("%s << Patches found after cluster filter = %d\n", __FUNCTION__, msers.size());
04398 }
04399
04400 if (DEBUG_MODE > 6)
04401 {
04402
04403 colorImage.copyTo(imCpy);
04404 drawContours(imCpy, msers, -1, color, 2);
04405 imshow("mainWin", imCpy);
04406 waitKey(0);
04407 }
04408
04409
04410 if (patches.size() > ((unsigned int)totalPatches))
04411 {
04412 reduceCluster(patches, msers, totalPatches);
04413 }
04414
04415
04416
04417
04418
04419 if (DEBUG_MODE > 1)
04420 {
04421 printf("%s << Patches remaining after area-based reduction = %d\n", __FUNCTION__, msers.size());
04422 }
04423
04424 if (DEBUG_MODE > 6)
04425 {
04426
04427 colorImage.copyTo(imCpy);
04428 drawContours(imCpy, msers, -1, color, 2);
04429 if (image.cols > 640)
04430 {
04431 resize(imCpy, dispMat, Size(0,0), 0.5, 0.5);
04432 imshow("mainWin", dispMat);
04433 }
04434 else
04435 {
04436 imshow("mainWin", imCpy);
04437 }
04438 waitKey(0);
04439 }
04440
04441 if (DEBUG_MODE > 0)
04442 {
04443 t = getTickCount() - t;
04444 printf("%s << Cluster Filter duration: %fms\n", __FUNCTION__, t*1000/getTickFrequency());
04445 }
04446
04447
04448
04449
04450
04451
04452 }
04453
04454 if (DEBUG_MODE > 3)
04455 {
04456 color = Scalar(255, 0, 0);
04457 colorImage.copyTo(imCpy);
04458 drawContours(imCpy, msers, -1, color, 2);
04459 if (colorImage.cols > 640)
04460 {
04461 resize(imCpy, dispMat, Size(0,0), 0.5, 0.5);
04462 imshow("mainWin", dispMat);
04463 }
04464 else
04465 {
04466 imshow("mainWin", imCpy);
04467 }
04468 waitKey(0);
04469 }
04470
04471
04472
04473 if (int(msers.size()) == totalPatches)
04474 {
04475
04476
04477 if (DEBUG_MODE > 1)
04478 {
04479 printf("%s << Pattern believed to be found.\n", __FUNCTION__);
04480 }
04481
04482
04483
04484
04485 for (unsigned int i = 0; i < patches.size(); i++)
04486 {
04487 patchCentres.push_back(patches.at(i).centroid2f);
04488 }
04489
04490
04491
04492
04493 return true;
04494 }
04495 else if (msers.size() > ((unsigned int)totalPatches))
04496 {
04497 if (DEBUG_MODE > 1)
04498 {
04499 printf("%s << Too many final patches = %d/%d\n", __FUNCTION__, msers.size(), totalPatches);
04500 }
04501 return false;
04502 }
04503 else
04504 {
04505 if (DEBUG_MODE > 1)
04506 {
04507 printf("%s << Too few final patches: %d/%d\n", __FUNCTION__, msers.size(), totalPatches);
04508 }
04509 return false;
04510 }
04511 }