00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00024
00025 #include <featuretracker.h>
00026
00027 #include <qvprojective.h>
00028 #include <QVCombinationIterator>
00029
00030 #ifndef DOXYGEN_IGNORE_THIS
00031 bool testCoherentMatchings(const QList< QPair<QPointF, QPointF> > &matchings)
00032 {
00033 const QList<QPointF> sourcePoints = getFirstPairList<QPointF>(matchings),
00034 destinationPoints = getSecondPairList<QPointF>(matchings);
00035
00036
00037 foreach (QPointF point, sourcePoints)
00038 if (sourcePoints.count(point) > 1)
00039 return false;
00040
00041 foreach (QPointF point, destinationPoints)
00042 if (destinationPoints.count(point) > 1)
00043 return false;
00044
00045 return true;
00046 }
00047
00048 bool testCoherentMatchings(const QList< QPair<QPointF, QPointF> > &matchings, const QPair<QPointF, QPointF> matching)
00049 {
00050 QPair <QPointF, QPointF> inlierMatching;
00051 foreach (inlierMatching, matchings)
00052 if (inlierMatching.first == matching.first || inlierMatching.second == matching.second)
00053 return false;
00054
00055 return true;
00056 }
00057
00058
00059
00060
00061
00062
00063
00064
00065
00066
00067
00068
00069
00070
00071
00072
00073
00074
00075
00076
00077
00078
00079
00080
00081
00082
00083
00084
00085
00086
00087
00088
00089
00090
00091
00092
00093
00094
00095
00096
00097
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
00130
00131
00132 template <typename Element, typename Model> class QVPROSAC2: public QVPROSAC<Element, Model>
00133 {
00134 protected:
00135 QVCombinationIterator combination;
00136
00137 virtual const QList< Element > getNextMaybeInliersSet()
00138 {
00139 QList< Element > result;
00140
00141 for (int i = 0; i < this->combination.getSubsetsSize(); i++)
00142 result.append(this->elements.at(this->combination[i]));
00143 this->continueCondition = this->combination.increment();
00144
00145 return result;
00146 }
00147
00148 bool init()
00149 {
00150 if (QVPROSAC<Element, Model>::init())
00151 this->combination = QVCombinationIterator(this->elements.size(), this->sampleSetSize);
00152 else
00153 return false;
00154
00155 return true;
00156 }
00157
00158 public:
00159 QVPROSAC2(const int sampleSetSize, const int minInliers): QVPROSAC<Element, Model>(sampleSetSize, minInliers) { }
00160 };
00161
00162 class QVEuclideanPROSAC2: public QVPROSAC< QPair<QPointF, QPointF>, QVMatrix>
00163 {
00164 private:
00165 double maxPixelDist;
00166
00167 public:
00168 QVEuclideanPROSAC2( const QList<QPointF> &sourcePoints, const QList<QPointF> &destinationPoints,
00169
00170 const double maxPixelDist, const int minTestInliers, const QVMatrix previousH):
00171 QVPROSAC< QPair<QPointF, QPointF>, QVMatrix>(2,minTestInliers), maxPixelDist(maxPixelDist)
00172 {
00173 for (int i = sourcePoints.size()-1; i >= 0 ; i--)
00174 for (int j = destinationPoints.size()-1; j >= 0 ; j--)
00175 {
00176 const QPointF source = sourcePoints.at(i),
00177 destination = destinationPoints.at(j);
00178
00179 addElement( QPair<QPointF, QPointF>(source, destination),
00180 norm2(ApplyHomography(previousH, source) - destination)
00181 );
00182 }
00183
00184
00185
00186
00187
00188
00189
00190
00191
00192
00193
00194
00195
00196
00197
00198
00199
00200
00201
00202
00203
00204
00205
00206
00207
00208
00209
00210
00211
00212
00213
00214
00215
00216
00217
00218
00219
00220
00221 }
00222
00223 const bool fit(const QList< QPair<QPointF, QPointF> > &matchings, QVMatrix &homography)
00224 {
00225 if (!testCoherentMatchings(matchings))
00226 return false;
00227
00228 homography = ComputeEuclideanHomography(matchings);
00229
00230 return true;
00231 };
00232
00233 const bool test(const QVMatrix &homography, const QPair<QPointF, QPointF> &matching)
00234 {
00235 if (!testCoherentMatchings(inliersSet, matching))
00236 return false;
00237
00238 return norm2(ApplyHomography(homography, matching.first) - matching.second) < maxPixelDist;
00239 };
00240 };
00241 #endif
00242
00244
00245 FeatureEuclideanTracker::FeatureEuclideanTracker(QString name): QVWorker(name), ransacIterations(0), varIterations(0), ignoredFrames(0), previousH(QVMatrix::identity(3))
00246 {
00247 addProperty<double>("Max pixel dist", inputFlag, 1.0, "For a pixel considered to be coincident", 0.0, 100.0);
00248 addProperty<int>("Max iterations", inputFlag, 150, "Maximal number of iterations to perform PROSAC", 1, 1000);
00249 addProperty<int>("Maximal test inliers", inputFlag, 40, "Number of points to perform RANSAC search per frame", 10, 100);
00250 addProperty<int>("Minimal test inliers", inputFlag, 15, "Minimal number of matchings for a model to be considered appropiated", 3, 30);
00251
00252 addProperty<int>("Skip unmatched frames", inputFlag, 10, "Number of frames with no correspondences to skip", 0, 50);
00253
00254 addProperty< QVMatrix >("Euclidean transformation", outputFlag, QVMatrix::identity(3));
00255
00256 addProperty< QList < QPointF > >("Feature locations", inputFlag);
00257
00258
00259 addProperty< CulebrillaContainer >("Culebrillas container", outputFlag);
00260 }
00261
00262 void FeatureEuclideanTracker::iterate()
00263 {
00264
00265 const int maxIterations = getPropertyValue<int>("Max iterations"),
00266 maximalTestInliers = getPropertyValue<int>("Maximal test inliers"),
00267 minimalTestInliers = getPropertyValue<int>("Minimal test inliers"),
00268
00269 maxSkipFrames = getPropertyValue<int>("Skip unmatched frames");
00270 const double maxPixelDist = getPropertyValue<double>("Max pixel dist");
00271
00272 timeFlag("Read input properties");
00273
00274
00275
00276 const QList<QPointF> actualPoints = getPropertyValue< QList< QPointF > >("Feature locations");
00277
00278 timeFlag("Get maximal hot points");
00279
00280
00281 bool matchingFound = false;
00282
00283 QList< QPair<QPointF, QPointF> > bestInliers;
00284
00285 if (actualPoints.size() >= 4)
00286 {
00287 const QList<QPointF> reducedActualPoints = actualPoints.mid(MAX(0, (int) actualPoints.size() - maximalTestInliers));
00288 const QList<QPointF> reducedLastPoints = lastPoints.mid(MAX(0, (int)lastPoints.size() - maximalTestInliers));
00289
00290
00291
00292 QVEuclideanPROSAC2 sampler(reducedLastPoints, reducedActualPoints, maxPixelDist, minimalTestInliers, previousH);
00293
00294 if (matchingFound = sampler.iterate(maxIterations))
00295 {
00296 const QList< QPair<QPointF, QPointF> > finalInliers = sampler.getBestInliers();
00297 setPropertyValue< QVMatrix > ("Euclidean transformation", previousH = sampler.getBestModel());
00298
00299
00300
00301
00302
00303
00304
00305
00306
00307
00308
00309
00310
00311
00312
00313
00314
00315
00316
00317
00318
00319
00320
00321 QPair< QPointF, QPointF > matching;
00322 foreach (matching, finalInliers)
00323 culebrillas.addMatching(matching.first, matching.second);
00324 setPropertyValue< CulebrillaContainer > ("Culebrillas container", culebrillas);
00325 }
00326 else {
00327 ignoredFrames++;
00328 setPropertyValue< QVMatrix > ("Euclidean transformation", QVMatrix::identity(3));
00329 }
00330
00331 const int iteration = getIteration() +1;
00332 ransacIterations += sampler.getIterations();
00333 varIterations += ABS(ransacIterations/iteration - sampler.getIterations());
00334 }
00335
00336 timeFlag("Sample consensus");
00337
00338 if (matchingFound || ignoredFrames >= maxSkipFrames)
00339 {
00340
00341 culebrillas.step();
00342
00343
00344 lastPoints = actualPoints;
00345
00346
00347 ignoredFrames = 0;
00348 if (!matchingFound)
00349 std::cout << "** ERROR: LOST TRACK OF POINTS at iteration " << getIteration() << " **" << std::endl;
00350 }
00351
00352 timeFlag("Get tracking points");
00353 }
00354
00356
00357 FeatureEuclideanMatcher::FeatureEuclideanMatcher(QString name): QVWorker(name), ransacIterations(0), varIterations(0), ignoredFrames(0), previousH(QVMatrix::identity(3))
00358 {
00359 addProperty<double>("Max pixel dist", inputFlag, 1.0, "For a pixel considered to be coincident", 0.0, 100.0);
00360
00361
00362
00363
00364
00365
00366 addProperty< QVMatrix >("Euclidean transformation", inputFlag, QVMatrix::identity(1));
00367
00368 addProperty< QList < QPointF > >("Feature locations", inputFlag);
00369
00370
00371 addProperty< CulebrillaContainer >("Culebrillas container", outputFlag);
00372 }
00373
00374 void FeatureEuclideanMatcher::iterate()
00375 {
00376
00377 const int
00378
00379
00380
00381
00382 const double maxPixelDist = getPropertyValue<double>("Max pixel dist");
00383
00384 timeFlag("Read input properties");
00385
00386
00387
00388 const QList<QPointF> actualPoints = getPropertyValue< QList< QPointF > >("Feature locations");
00389
00390 timeFlag("Get maximal hot points");
00391
00392
00393 bool matchingFound = false;
00394
00395 QList< QPair<QPointF, QPointF> > bestInliers;
00396
00397 if (actualPoints.size() >= 4)
00398 {
00399 const QList<QPointF> reducedActualPoints = actualPoints.mid(MAX(0, (int) actualPoints.size() - maximalTestInliers));
00400 const QList<QPointF> reducedLastPoints = lastPoints.mid(MAX(0, (int)lastPoints.size() - maximalTestInliers));
00401
00402
00403
00404 QVEuclideanPROSAC2 sampler(reducedLastPoints, reducedActualPoints, maxPixelDist, minimalTestInliers, previousH);
00405
00406 if (matchingFound = sampler.iterate(maxIterations))
00407 {
00408 const QList< QPair<QPointF, QPointF> > finalInliers = sampler.getBestInliers();
00409 setPropertyValue< QVMatrix > ("Euclidean transformation", previousH = sampler.getBestModel());
00410
00411
00412
00413
00414
00415
00416
00417
00418
00419
00420
00421
00422
00423
00424
00425
00426
00427
00428
00429
00430
00431
00432
00433 QPair< QPointF, QPointF > matching;
00434 foreach (matching, finalInliers)
00435 culebrillas.addMatching(matching.first, matching.second);
00436 setPropertyValue< CulebrillaContainer > ("Culebrillas container", culebrillas);
00437 }
00438 else {
00439 ignoredFrames++;
00440 setPropertyValue< QVMatrix > ("Euclidean transformation", QVMatrix::identity(3));
00441 }
00442
00443 const int iteration = getIteration() +1;
00444 ransacIterations += sampler.getIterations();
00445 varIterations += ABS(ransacIterations/iteration - sampler.getIterations());
00446 }
00447
00448 timeFlag("Sample consensus");
00449
00450 if (matchingFound || ignoredFrames >= maxSkipFrames)
00451 {
00452
00453 culebrillas.step();
00454
00455
00456 lastPoints = actualPoints;
00457
00458
00459 ignoredFrames = 0;
00460 if (!matchingFound)
00461 std::cout << "** ERROR: LOST TRACK OF POINTS at iteration " << getIteration() << " **" << std::endl;
00462 }
00463
00464 timeFlag("Get tracking points");
00465 }
00466