00001
00002
00003
00004
00005
00006
00007 #include "ColorFilter.h"
00008 #include "DocumentModelPointMatch.h"
00009 #include "EngaugeAssert.h"
00010 #include <iostream>
00011 #include "Logger.h"
00012 #include "PointMatchAlgorithm.h"
00013 #include <QFile>
00014 #include <QImage>
00015 #include <qmath.h>
00016 #include <QTextStream>
00017
00018 using namespace std;
00019
00020 #define FOLD2DINDEX(i,j,jmax) ((i)*(jmax)+j)
00021
00022 const int PIXEL_OFF = -1;
00023
00024 const int PIXEL_ON = 1;
00025
00026 PointMatchAlgorithm::PointMatchAlgorithm(bool isGnuplot) :
00027 m_isGnuplot (isGnuplot)
00028 {
00029 LOG4CPP_INFO_S ((*mainCat)) << "PointMatchAlgorithm::PointMatchAlgorithm";
00030 }
00031
00032 void PointMatchAlgorithm::allocateMemory(double** array,
00033 fftw_complex** arrayPrime,
00034 int width,
00035 int height)
00036 {
00037 LOG4CPP_INFO_S ((*mainCat)) << "PointMatchAlgorithm::allocateMemory";
00038
00039 *array = new double [width * height];
00040 ENGAUGE_CHECK_PTR(*array);
00041
00042 *arrayPrime = new fftw_complex [width * height];
00043 ENGAUGE_CHECK_PTR(*arrayPrime);
00044 }
00045
00046 void PointMatchAlgorithm::assembleLocalMaxima(double* convolution,
00047 PointMatchList& listCreated,
00048 int width,
00049 int height)
00050 {
00051 LOG4CPP_INFO_S ((*mainCat)) << "PointMatchAlgorithm::assembleLocalMaxima";
00052
00053
00054 const double SINGLE_PIXEL_CORRELATION = 1.0;
00055
00056 for (int i = 0; i < width; i++) {
00057 for (int j = 0; j < height; j++) {
00058
00059
00060 double convIJ = log10 (convolution[FOLD2DINDEX(i, j, height)]);
00061
00062
00063 bool isLocalMax = true;
00064 for (int iDelta = -1; (iDelta <= 1) && isLocalMax; iDelta++) {
00065
00066 int iNeighbor = i + iDelta;
00067 if ((0 <= iNeighbor) && (iNeighbor < width)) {
00068
00069 for (int jDelta = -1; (jDelta <= 1) && isLocalMax; jDelta++) {
00070
00071 int jNeighbor = j + jDelta;
00072 if ((0 <= jNeighbor) && (jNeighbor < height)) {
00073
00074
00075 double convNeighbor = log10 (convolution[FOLD2DINDEX(iNeighbor, jNeighbor, height)]);
00076 if (convIJ < convNeighbor) {
00077
00078 isLocalMax = false;
00079
00080 } else if (convIJ == convNeighbor) {
00081
00082
00083 if ((jDelta < 0) || (jDelta == 0 && iDelta < 0)) {
00084
00085 isLocalMax = false;
00086 }
00087 }
00088 }
00089 }
00090 }
00091 }
00092
00093 if (isLocalMax &&
00094 (convIJ > SINGLE_PIXEL_CORRELATION) ) {
00095
00096
00097 PointMatchTriplet t (i,
00098 j,
00099 convolution [FOLD2DINDEX(i, j, height)]);
00100
00101 listCreated.append(t);
00102 }
00103 }
00104 }
00105 }
00106
00107 void PointMatchAlgorithm::computeConvolution(fftw_complex* imagePrime,
00108 fftw_complex* samplePrime,
00109 int width, int height,
00110 double** convolution,
00111 int sampleXCenter,
00112 int sampleYCenter)
00113 {
00114 LOG4CPP_INFO_S ((*mainCat)) << "PointMatchAlgorithm::computeConvolution";
00115
00116 fftw_complex* convolutionPrime;
00117
00118 allocateMemory(convolution,
00119 &convolutionPrime,
00120 width,
00121 height);
00122
00123
00124 conjugateMatrix(width,
00125 height,
00126 samplePrime);
00127
00128
00129 multiplyMatrices(width,
00130 height,
00131 imagePrime,
00132 samplePrime,
00133 convolutionPrime);
00134
00135
00136 fftw_plan pConvolution = fftw_plan_dft_c2r_2d(width,
00137 height,
00138 convolutionPrime,
00139 *convolution,
00140 FFTW_ESTIMATE);
00141
00142 fftw_execute (pConvolution);
00143
00144 releasePhaseArray(convolutionPrime);
00145
00146
00147
00148 double *temp = new double [width * height];
00149 ENGAUGE_CHECK_PTR(temp);
00150
00151 for (int i = 0; i < width; i++) {
00152 for (int j = 0; j < height; j++) {
00153 temp [FOLD2DINDEX(i, j, height)] = (*convolution) [FOLD2DINDEX(i, j, height)];
00154 }
00155 }
00156 for (int iFrom = 0; iFrom < width; iFrom++) {
00157 for (int jFrom = 0; jFrom < height; jFrom++) {
00158
00159 int iTo = (iFrom + sampleXCenter) % width;
00160 int jTo = (jFrom + sampleYCenter) % height;
00161 (*convolution) [FOLD2DINDEX(iTo, jTo, height)] = temp [FOLD2DINDEX(iFrom, jFrom, height)];
00162 }
00163 }
00164 delete [] temp;
00165 }
00166
00167 void PointMatchAlgorithm::conjugateMatrix(int width,
00168 int height,
00169 fftw_complex* matrix)
00170 {
00171 LOG4CPP_INFO_S ((*mainCat)) << "PointMatchAlgorithm::conjugateMatrix";
00172
00173 ENGAUGE_CHECK_PTR(matrix);
00174
00175 for (int x = 0; x < width; x++) {
00176 for (int y = 0; y < height; y++) {
00177
00178 int index = FOLD2DINDEX(x, y, height);
00179 matrix [index] [1] = -1.0 * matrix [index] [1];
00180 }
00181 }
00182 }
00183
00184 void PointMatchAlgorithm::dumpToGnuplot (double* convolution,
00185 int width,
00186 int height,
00187 const QString &filename) const
00188 {
00189 LOG4CPP_INFO_S ((*mainCat)) << "PointMatchAlgorithm::dumpToGnuplot";
00190
00191 cout << "Writing gnuplot file: " << filename.toLatin1().data() << "\n";
00192
00193 QFile file (filename);
00194 if (file.open (QIODevice::WriteOnly | QIODevice::Text)) {
00195
00196 QTextStream str (&file);
00197
00198 str << "# Suggested gnuplot commands:" << endl;
00199 str << "# set hidden3d" << endl;
00200 str << "# splot \"" << filename << "\" u 1:2:3 with pm3d" << endl;
00201 str << endl;
00202
00203 str << "# I J Convolution" << endl;
00204 for (int i = 0; i < width; i++) {
00205 for (int j = 0; j < height; j++) {
00206
00207 double convIJ = convolution[FOLD2DINDEX(i, j, height)];
00208 str << i << " " << j << " " << convIJ << endl;
00209 }
00210 str << endl;
00211 }
00212 }
00213
00214 file.close();
00215 }
00216
00217 QList<QPoint> PointMatchAlgorithm::findPoints (const QList<PointMatchPixel> &samplePointPixels,
00218 const QImage &imageProcessed,
00219 const DocumentModelPointMatch &modelPointMatch,
00220 const Points &pointsExisting)
00221 {
00222 LOG4CPP_INFO_S ((*mainCat)) << "PointMatchAlgorithm::findPoints"
00223 << " samplePointPixels=" << samplePointPixels.count();
00224
00225
00226 int originalWidth = imageProcessed.width();
00227 int originalHeight = imageProcessed.height();
00228 int width = optimizeLengthForFft(originalWidth);
00229 int height = optimizeLengthForFft(originalHeight);
00230
00231
00232
00233 double *image, *sample, *convolution;
00234 fftw_complex *imagePrime, *samplePrime;
00235
00236
00237 int sampleXCenter, sampleYCenter, sampleXExtent, sampleYExtent;
00238 loadImage(imageProcessed,
00239 modelPointMatch,
00240 pointsExisting,
00241 width,
00242 height,
00243 &image,
00244 &imagePrime);
00245 loadSample(samplePointPixels,
00246 width,
00247 height,
00248 &sample,
00249 &samplePrime,
00250 &sampleXCenter,
00251 &sampleYCenter,
00252 &sampleXExtent,
00253 &sampleYExtent);
00254 computeConvolution(imagePrime,
00255 samplePrime,
00256 width,
00257 height,
00258 &convolution,
00259 sampleXCenter,
00260 sampleYCenter);
00261
00262 if (m_isGnuplot) {
00263
00264 dumpToGnuplot(image,
00265 width,
00266 height,
00267 "image.gnuplot");
00268 dumpToGnuplot(sample,
00269 width,
00270 height,
00271 "sample.gnuplot");
00272 dumpToGnuplot(convolution,
00273 width,
00274 height,
00275 "convolution.gnuplot");
00276 }
00277
00278
00279
00280 PointMatchList listCreated;
00281 assembleLocalMaxima(convolution,
00282 listCreated,
00283 width,
00284 height);
00285 qSort (listCreated);
00286
00287
00288 QList<QPoint> pointsCreated;
00289 PointMatchList::iterator itr;
00290 for (itr = listCreated.begin(); itr != listCreated.end(); itr++) {
00291
00292 PointMatchTriplet triplet = *itr;
00293 pointsCreated.push_back (triplet.point ());
00294
00295
00296
00297
00298
00299 }
00300
00301 releaseImageArray(image);
00302 releasePhaseArray(imagePrime);
00303 releaseImageArray(sample);
00304 releasePhaseArray(samplePrime);
00305 releaseImageArray(convolution);
00306
00307 return pointsCreated;
00308 }
00309
00310 void PointMatchAlgorithm::loadImage(const QImage &imageProcessed,
00311 const DocumentModelPointMatch &modelPointMatch,
00312 const Points &pointsExisting,
00313 int width,
00314 int height,
00315 double** image,
00316 fftw_complex** imagePrime)
00317 {
00318 LOG4CPP_INFO_S ((*mainCat)) << "PointMatchAlgorithm::loadImage";
00319
00320 allocateMemory(image,
00321 imagePrime,
00322 width,
00323 height);
00324
00325 populateImageArray(imageProcessed,
00326 width,
00327 height,
00328 image);
00329
00330 removePixelsNearExistingPoints(*image,
00331 width,
00332 height,
00333 pointsExisting,
00334 modelPointMatch.maxPointSize());
00335
00336
00337 fftw_plan pImage = fftw_plan_dft_r2c_2d(width,
00338 height,
00339 *image,
00340 *imagePrime,
00341 FFTW_ESTIMATE);
00342 fftw_execute(pImage);
00343 }
00344
00345 void PointMatchAlgorithm::loadSample(const QList<PointMatchPixel> &samplePointPixels,
00346 int width,
00347 int height,
00348 double** sample,
00349 fftw_complex** samplePrime,
00350 int* sampleXCenter,
00351 int* sampleYCenter,
00352 int* sampleXExtent,
00353 int* sampleYExtent)
00354 {
00355 LOG4CPP_INFO_S ((*mainCat)) << "PointMatchAlgorithm::loadImage";
00356
00357
00358
00359 allocateMemory(sample,
00360 samplePrime,
00361 width,
00362 height);
00363
00364 populateSampleArray(samplePointPixels,
00365 width,
00366 height,
00367 sample,
00368 sampleXCenter,
00369 sampleYCenter,
00370 sampleXExtent,
00371 sampleYExtent);
00372
00373
00374 fftw_plan pSample = fftw_plan_dft_r2c_2d(width,
00375 height,
00376 *sample,
00377 *samplePrime,
00378 FFTW_ESTIMATE);
00379 fftw_execute(pSample);
00380 }
00381
00382 void PointMatchAlgorithm::multiplyMatrices(int width,
00383 int height,
00384 fftw_complex* in1,
00385 fftw_complex* in2,
00386 fftw_complex* out)
00387 {
00388 LOG4CPP_INFO_S ((*mainCat)) << "PointMatchAlgorithm::multiplyMatrices";
00389
00390 for (int x = 0; x < width; x++) {
00391 for (int y = 0; y < height; y++) {
00392
00393 int index = FOLD2DINDEX(x, y, height);
00394
00395 out [index] [0] = in1 [index] [0] * in2 [index] [0] - in1 [index] [1] * in2 [index] [1];
00396 out [index] [1] = in1 [index] [0] * in2 [index] [1] + in1 [index] [1] * in2 [index] [0];
00397 }
00398 }
00399 }
00400
00401 int PointMatchAlgorithm::optimizeLengthForFft(int originalLength)
00402 {
00403
00404
00405 const int INITIAL_CLOSEST_LENGTH = 0;
00406
00407
00408
00409
00410 int closestLength = INITIAL_CLOSEST_LENGTH;
00411 for (int power2 = 1; power2 < originalLength; power2 *= 2) {
00412 for (int power3 = 1; power3 < originalLength; power3 *= 3) {
00413 for (int power5 = 1; power5 < originalLength; power5 *= 5) {
00414 for (int power7 = 1; power7 < originalLength; power7 *= 7) {
00415
00416 int newLength = power2 * power3 * power5 * power7;
00417 if (originalLength <= newLength) {
00418
00419 if ((closestLength == INITIAL_CLOSEST_LENGTH) ||
00420 (newLength < closestLength)) {
00421
00422
00423
00424 closestLength = newLength;
00425 }
00426 }
00427 }
00428 }
00429 }
00430 }
00431
00432 if (closestLength == INITIAL_CLOSEST_LENGTH) {
00433
00434
00435 closestLength = originalLength;
00436 }
00437
00438 LOG4CPP_INFO_S ((*mainCat)) << "PointMatchAlgorithm::optimizeLengthForFft"
00439 << " originalLength=" << originalLength
00440 << " newLength=" << closestLength;
00441
00442 return closestLength;
00443 }
00444
00445 void PointMatchAlgorithm::populateImageArray(const QImage &imageProcessed,
00446 int width,
00447 int height,
00448 double** image)
00449 {
00450 LOG4CPP_INFO_S ((*mainCat)) << "PointMatchAlgorithm::populateImageArray";
00451
00452
00453 ColorFilter colorFilter;
00454 for (int x = 0; x < width; x++) {
00455 for (int y = 0; y < height; y++) {
00456 bool pixelIsOn = colorFilter.pixelFilteredIsOn (imageProcessed,
00457 x,
00458 y);
00459
00460 (*image) [FOLD2DINDEX(x, y, height)] = (pixelIsOn ?
00461 PIXEL_ON :
00462 PIXEL_OFF);
00463 }
00464 }
00465 }
00466
00467 void PointMatchAlgorithm::populateSampleArray(const QList<PointMatchPixel> &samplePointPixels,
00468 int width,
00469 int height,
00470 double** sample,
00471 int* sampleXCenter,
00472 int* sampleYCenter,
00473 int* sampleXExtent,
00474 int* sampleYExtent)
00475 {
00476 LOG4CPP_INFO_S ((*mainCat)) << "PointMatchAlgorithm::populateSampleArray";
00477
00478
00479 bool first = true;
00480 unsigned int i;
00481 int xMin = width, yMin = height, xMax = 0, yMax = 0;
00482 for (i = 0; i < (unsigned int) samplePointPixels.size(); i++) {
00483
00484 int x = (samplePointPixels.at(i)).xOffset();
00485 int y = (samplePointPixels.at(i)).yOffset();
00486 if (first || (x < xMin))
00487 xMin = x;
00488 if (first || (x > xMax))
00489 xMax = x;
00490 if (first || (y < yMin))
00491 yMin = y;
00492 if (first || (y > yMax))
00493 yMax = y;
00494
00495 first = false;
00496 }
00497
00498 const int border = 1;
00499
00500 xMin -= border;
00501 yMin -= border;
00502 xMax += border;
00503 yMax += border;
00504
00505
00506 int x, y;
00507 for (x = 0; x < width; x++) {
00508 for (y = 0; y < height; y++) {
00509 (*sample) [FOLD2DINDEX(x, y, height)] = PIXEL_OFF;
00510 }
00511 }
00512
00513
00514
00515
00516 double xSumOn = 0, ySumOn = 0, countOn = 0;
00517
00518 for (i = 0; i < (unsigned int) samplePointPixels.size(); i++) {
00519
00520
00521 x = (samplePointPixels.at(i)).xOffset() - xMin;
00522 y = (samplePointPixels.at(i)).yOffset() - yMin;
00523 ENGAUGE_ASSERT((0 < x) && (x < width));
00524 ENGAUGE_ASSERT((0 < y) && (y < height));
00525
00526 bool pixelIsOn = samplePointPixels.at(i).pixelIsOn();
00527
00528 (*sample) [FOLD2DINDEX(x, y, height)] = (pixelIsOn ? PIXEL_ON : PIXEL_OFF);
00529
00530 if (pixelIsOn) {
00531 xSumOn += x;
00532 ySumOn += y;
00533 ++countOn;
00534 }
00535 }
00536
00537
00538 countOn = qMax (1.0, countOn);
00539 *sampleXCenter = (int) (0.5 + xSumOn / countOn);
00540 *sampleYCenter = (int) (0.5 + ySumOn / countOn);
00541
00542
00543 *sampleXExtent = xMax - xMin + 1;
00544 *sampleYExtent = yMax - yMin + 1;
00545 }
00546
00547 void PointMatchAlgorithm::releaseImageArray(double* array)
00548 {
00549 LOG4CPP_INFO_S ((*mainCat)) << "PointMatchAlgorithm::releaseImageArray";
00550
00551 ENGAUGE_CHECK_PTR(array);
00552 delete[] array;
00553 }
00554
00555 void PointMatchAlgorithm::releasePhaseArray(fftw_complex* arrayPrime)
00556 {
00557 LOG4CPP_INFO_S ((*mainCat)) << "PointMatchAlgorithm::releasePhaseArray";
00558
00559 ENGAUGE_CHECK_PTR(arrayPrime);
00560 delete[] arrayPrime;
00561 }
00562
00563 void PointMatchAlgorithm::removePixelsNearExistingPoints(double* image,
00564 int imageWidth,
00565 int imageHeight,
00566 const Points &pointsExisting,
00567 int pointSeparation)
00568 {
00569 LOG4CPP_INFO_S ((*mainCat)) << "PointMatchAlgorithm::removePixelsNearExistingPoints";
00570
00571 for (int i = 0; i < pointsExisting.size(); i++) {
00572
00573 int xPoint = pointsExisting.at(i).posScreen().x();
00574 int yPoint = pointsExisting.at(i).posScreen().y();
00575
00576
00577 int yMin = yPoint - pointSeparation;
00578 if (yMin < 0)
00579 yMin = 0;
00580 int yMax = yPoint + pointSeparation;
00581 if (imageHeight < yMax)
00582 yMax = imageHeight;
00583
00584 for (int y = yMin; y < yMax; y++) {
00585
00586
00587 int radical = pointSeparation * pointSeparation - (y - yPoint) * (y - yPoint);
00588 if (0 < radical) {
00589
00590 int xMin = (int) (xPoint - qSqrt((double) radical));
00591 if (xMin < 0)
00592 xMin = 0;
00593 int xMax = xPoint + (xPoint - xMin);
00594 if (imageWidth < xMax)
00595 xMax = imageWidth;
00596
00597
00598 for (int x = xMin; x < xMax; x++) {
00599
00600 image [FOLD2DINDEX(x, y, imageHeight)] = PIXEL_OFF;
00601
00602 }
00603 }
00604 }
00605 }
00606 }