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 <qvmath/qvprojective.h>
00028 #include <qvmath/qvcombinationiterator.h>
00029
00030 bool testCoherentMatchings(const QList< QPair<QPointF, QPointF> > &matchings)
00031 {
00032 const QList<QPointF> sourcePoints = getFirstPairList<QPointF>(matchings),
00033 destinationPoints = getSecondPairList<QPointF>(matchings);
00034
00035
00036 foreach (QPointF point, sourcePoints)
00037 if (sourcePoints.count(point) > 1)
00038 return false;
00039
00040 foreach (QPointF point, destinationPoints)
00041 if (destinationPoints.count(point) > 1)
00042 return false;
00043
00044 return true;
00045 }
00046
00047 bool testCoherentMatchings(const QList< QPair<QPointF, QPointF> > &matchings, const QPair<QPointF, QPointF> matching)
00048 {
00049 QPair <QPointF, QPointF> inlierMatching;
00050 foreach (inlierMatching, matchings)
00051 if (inlierMatching.first == matching.first || inlierMatching.second == matching.second)
00052 return false;
00053
00054 return true;
00055 }
00056
00057 #ifndef DOXYGEN_IGNORE_THIS
00058 class QVEuclideanRANSAC: public QVRANSAC< QPair<QPointF, QPointF>, QVMatrix>
00059 {
00060 private:
00061 double maxPixelDist;
00062
00063 public:
00064 QVEuclideanRANSAC( const QList<QPointF> &sourcePoints, const QList<QPointF> &destinationPoints,
00065 const double maxPixelDist, const double maxHomographyDistance, const int minTestInliers):
00066 QVRANSAC< QPair<QPointF, QPointF>, QVMatrix>(2,minTestInliers), maxPixelDist(maxPixelDist)
00067 {
00068 foreach(QPointF source, sourcePoints)
00069 foreach(QPointF destination, destinationPoints)
00070 addElement(QPair<QPointF, QPointF>(source, destination));
00071
00072 }
00073
00074 const bool fit(const QList< QPair<QPointF, QPointF> > &matchings, QVMatrix &homography)
00075 {
00076 if (!testCoherentMatchings(matchings))
00077 return false;
00078
00079 homography = ComputeEuclideanHomography(matchings);
00080
00081 return true;
00082 };
00083
00084 const bool test(const QVMatrix &homography, const QPair<QPointF, QPointF> &matching)
00085 {
00086 if (!testCoherentMatchings(inliersSet, matching))
00087 return false;
00088
00089 return norm2(ApplyHomography(homography, matching.first) - matching.second) < maxPixelDist;
00090 };
00091 };
00092
00093
00094
00095 class QVEuclideanPROSAC: public QVPROSAC< QPair<QPointF, QPointF>, QVMatrix>
00096 {
00097 private:
00098 double maxPixelDist;
00099
00100 public:
00101 QVEuclideanPROSAC( const QList<QPointF> &sourcePoints, const QList<QPointF> &destinationPoints,
00102 const QList<sFloat> &sourceResponses, const QList<sFloat> &destinationResponses,
00103 const double maxPixelDist, const double maxHomographyDistance, const int minTestInliers):
00104 QVPROSAC< QPair<QPointF, QPointF>, QVMatrix>(2,minTestInliers), maxPixelDist(maxPixelDist)
00105 {
00106 for (int i = 0; i < sourcePoints.size(); i++)
00107 for (int j = 0; j < destinationPoints.size(); j++)
00108 addElement(QPair<QPointF, QPointF>(sourcePoints.at(i), destinationPoints.at(j)),
00109 1 / (ABS(sourceResponses.at(i)) * ABS(destinationResponses.at(j))) );
00110
00111
00112 }
00113
00114 const bool fit(const QList< QPair<QPointF, QPointF> > &matchings, QVMatrix &homography)
00115 {
00116 if (!testCoherentMatchings(matchings))
00117 return false;
00118
00119 homography = ComputeEuclideanHomography(matchings);
00120
00121 return true;
00122 };
00123
00124 const bool test(const QVMatrix &homography, const QPair<QPointF, QPointF> &matching)
00125 {
00126 if (!testCoherentMatchings(inliersSet, matching))
00127 return false;
00128
00129 return norm2(ApplyHomography(homography, matching.first) - matching.second) < maxPixelDist;
00130 };
00131 };
00132
00133 template <typename Element, typename Model> class QVPROSAC2: public QVPROSAC<Element, Model>
00134 {
00135 protected:
00136 QVCombinationIterator combination;
00137
00138 virtual const QList< Element > getNextMaybeInliersSet()
00139 {
00140 QList< Element > result;
00141
00142 for (int i = 0; i < this->combination.getSubsetsSize(); i++)
00143 result.append(this->elements.at(this->combination[i]));
00144 this->continueCondition = this->combination.increment();
00145
00146 return result;
00147 }
00148
00149 bool init()
00150 {
00151 if (QVPROSAC<Element, Model>::init())
00152 this->combination = QVCombinationIterator(this->elements.size(), this->sampleSetSize);
00153 else
00154 return false;
00155
00156 return true;
00157 }
00158
00159 public:
00160 QVPROSAC2(const int sampleSetSize, const int minInliers): QVPROSAC<Element, Model>(sampleSetSize, minInliers) { }
00161 };
00162
00163 class QVEuclideanPROSAC2: public QVPROSAC2< QPair<QPointF, QPointF>, QVMatrix>
00164 {
00165 private:
00166 double maxPixelDist;
00167
00168 public:
00169 QVEuclideanPROSAC2( const QList<QPointF> &sourcePoints, const QList<QPointF> &destinationPoints,
00170 const QList<sFloat> &sourceResponses, const QList<sFloat> &destinationResponses,
00171 const double maxPixelDist, const int minTestInliers):
00172 QVPROSAC2< QPair<QPointF, QPointF>, QVMatrix>(2,minTestInliers), maxPixelDist(maxPixelDist)
00173 {
00174
00175
00176
00177
00178
00179
00180
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 for (int i = 0; i < sourcePoints.size(); i++)
00208 for (int j = 0; j < destinationPoints.size(); j++)
00209 addElement(QPair<QPointF, QPointF>(sourcePoints.at(i), destinationPoints.at(j)),
00210 1 / (ABS(sourceResponses.at(i)) + ABS(destinationResponses.at(j)))
00211 );
00212
00213
00214 }
00215
00216 const bool fit(const QList< QPair<QPointF, QPointF> > &matchings, QVMatrix &homography)
00217 {
00218 if (!testCoherentMatchings(matchings))
00219 return false;
00220
00221 homography = ComputeEuclideanHomography(matchings);
00222
00223 return true;
00224 };
00225
00226 const bool test(const QVMatrix &homography, const QPair<QPointF, QPointF> &matching)
00227 {
00228 if (!testCoherentMatchings(inliersSet, matching))
00229 return false;
00230
00231 return norm2(ApplyHomography(homography, matching.first) - matching.second) < maxPixelDist;
00232 };
00233 };
00234 #endif
00235
00237
00238 FeatureTracker::FeatureTracker(QString name): QVWorker(name), ransacIterations(0), varIterations(0), ignoredFrames(0)
00239 {
00240 addProperty<double>("Max pixel dist", inputFlag, 0.01, "For a pixel considered to be coincident", 0.0, 0.1);
00241 addProperty<int>("Max iterations", inputFlag, 150, "Maximal number of iterations to perform PROSAC", 1, 1000);
00242 addProperty<int>("Maximal test inliers", inputFlag, 50, "Number of points to perform RANSAC search per frame", 10, 100);
00243 addProperty<int>("Minimal test inliers", inputFlag, 12, "Minimal number of matchings for a model to be considered appropiated", 3, 30);
00244 addProperty<int>("Maximal track points", inputFlag, 750, "Maximal points to track", 50, 5000);
00245 addProperty<int>("Skip unmatched frames", inputFlag, 10, "Number of frames with no correspondences to skip", 0, 50);
00246 addProperty< QVMatrix >("Euclidean transformation", outputFlag, QVMatrix::identity(3));
00247
00248 addProperty< QList < QPointF > >("Feature locations", inputFlag);
00249 addProperty< QList < sFloat > >("Feature responses", inputFlag);
00250
00251 addProperty< CulebrillaContainer >("Culebrillas container", outputFlag);
00252 }
00253
00254 void FeatureTracker::iterate()
00255 {
00256
00257 const int maxIterations = getPropertyValue<int>("Max iterations"),
00258 maximalTestInliers = getPropertyValue<int>("Maximal test inliers"),
00259 minimalTestInliers = getPropertyValue<int>("Minimal test inliers"),
00260 maximalTrackPoints = getPropertyValue<int>("Maximal track points"),
00261 maxSkipFrames = getPropertyValue<int>("Skip unmatched frames");
00262 const double maxPixelDist = getPropertyValue<double>("Max pixel dist");
00263
00264 timeFlag("Read input properties");
00265
00266
00267 const QList<sFloat> actualResponses = getPropertyValue< QList< sFloat > >("Feature responses");
00268 const QList<QPointF> actualPoints = getPropertyValue< QList< QPointF > >("Feature locations");
00269
00270 timeFlag("Get maximal hot points");
00271
00272
00273 bool matchingFound = false;
00274 QVMatrix Hfound = QVMatrix::identity(3);
00275
00276 if (actualPoints.size() >= 4)
00277 {
00278 const QList<QPointF> reducedActualPoints = actualPoints.mid(MAX(0, (int) actualPoints.size() - maximalTestInliers));
00279 const QList<QPointF> reducedLastPoints = lastPoints.mid(MAX(0, (int)lastPoints.size() - maximalTestInliers));
00280
00281
00282
00283 QVEuclideanPROSAC2 sampler(reducedLastPoints, reducedActualPoints, lastResponses, actualResponses, maxPixelDist, minimalTestInliers);
00284
00285 if (matchingFound = sampler.iterate(maxIterations))
00286 {
00287 Hfound = sampler.getBestModel();
00288 setPropertyValue< QVMatrix > ("Euclidean transformation", Hfound);
00289 }
00290 else
00291 setPropertyValue< QVMatrix > ("Euclidean transformation", QVMatrix::identity(3));
00292
00293 const int iteration = getIteration() +1;
00294 ransacIterations += sampler.getIterations();
00295 varIterations += ABS(ransacIterations/iteration - sampler.getIterations());
00296 }
00297
00298 timeFlag("Sample consensus");
00299
00300
00301
00302 if (matchingFound)
00303 {
00304
00305 const QList<QPointF> reducedLastPoints = lastPoints.mid(MAX(0, (int)lastPoints.size() - maximalTrackPoints));
00306 QList<QPointF> reducedActualPoints = actualPoints.mid(MAX(0, (int) actualPoints.size() - maximalTrackPoints));
00307
00308
00309
00310
00311 QList< QPair<QPointF, QPointF> > finalInliers;
00312 for (int i = reducedLastPoints.size()-1; i >= 0 ; i--)
00313 for (int j = reducedActualPoints.size()-1; j >= 0 ; j--)
00314 {
00315 const QPointF last = reducedLastPoints.at(i),
00316 actual = reducedActualPoints.at(j);
00317 if (norm2(ApplyHomography(Hfound, last) - actual) < maxPixelDist)
00318 {
00319 finalInliers.append(QPair<QPointF, QPointF>(last, actual));
00320 reducedActualPoints.removeAt(j);
00321 break;
00322 }
00323 }
00324
00325
00326 QPair< QPointF, QPointF > matching;
00327 foreach (matching, finalInliers)
00328 culebrillas.addMatching(matching.first, matching.second);
00329 setPropertyValue< CulebrillaContainer > ("Culebrillas container", culebrillas);
00330 }
00331 else {
00332
00333 ignoredFrames++;
00334 }
00335
00336 if (matchingFound || ignoredFrames >= maxSkipFrames)
00337 {
00338
00339 culebrillas.step();
00340
00341
00342 lastPoints = actualPoints;
00343 lastResponses = actualResponses;
00344
00345 ignoredFrames = 0;
00346 if (!matchingFound)
00347 std::cout << "** ERROR: LOST TRACK OF POINTS at iteration " << getIteration() << " **" << std::endl;
00348 }
00349 timeFlag("Get tracking points");
00350 }
00351