examples/featureTracker/featuretracker.cpp

00001 /*
00002  *      Copyright (C) 2007, 2008. PARP Research Group.
00003  *      <http://perception.inf.um.es>
00004  *      University of Murcia, Spain.
00005  *
00006  *      This file is part of the QVision library.
00007  *
00008  *      QVision is free software: you can redistribute it and/or modify
00009  *      it under the terms of the GNU Lesser General Public License as
00010  *      published by the Free Software Foundation, version 3 of the License.
00011  *
00012  *      QVision is distributed in the hope that it will be useful,
00013  *      but WITHOUT ANY WARRANTY; without even the implied warranty of
00014  *      MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00015  *      GNU Lesser General Public License for more details.
00016  *
00017  *      You should have received a copy of the GNU Lesser General Public
00018  *      License along with QVision. If not, see <http://www.gnu.org/licenses/>.
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         // Check every source point is matched with only one destination point, and viceversa.
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 /*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                         //init();
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 // Ransac class. This is useful to get matchings between points at the input image and the template image, and
00094 // obtain the matching homography if it can be found.
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                         //init();
00111                         }
00112 
00113                 const bool fit(const QList< QPair<QPointF, QPointF> > &matchings, QVMatrix &homography)
00114                         {
00115                         if (!testCoherentMatchings(matchings))
00116                                 return false;
00117 
00118                         homography = ComputeEuclideanHomography(matchings);
00119 
00120                         return true;
00121                         };
00122 
00123                 const bool test(const QVMatrix &homography, const QPair<QPointF, QPointF> &matching)
00124                         {
00125                         if (!testCoherentMatchings(inliersSet, matching))
00126                                 return false;
00127 
00128                         return norm2(ApplyHomography(homography, matching.first) - matching.second) < maxPixelDist;
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                                         /*const QList<sFloat> &sourceResponses, const QList<sFloat> &destinationResponses,*/
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                         Times and iterations for file 'http://perception.inf.um.es/public_data/videos/templates/plantillas-MSER/plantilla.avi'
00186                         For first 1000 input frames:
00187 
00188                                 Heuristic                               RANSAC time             Iterations per RANSAC
00189                                 ---------------                         ---------------         ---------------
00190                                 0 (sorted for corner response value)    8.92173                 625.286
00191                                 rand()                                  90.1418                 2506.99
00192                                 1 / (|s| + |d|)                         4.46567                 97.2883
00193                                 1 / (log(|s|) + log(|d|))               4.55425                 97.2893
00194                                 1 / (|s| * |d|)                         4.34896                 97.2492
00195                                 1 / |s - d|                             161.865                 4900.74
00196                                 |s - d|                                 67.087                  1831.34
00197                                 |s - d| / (|s| + |d|)                   66.7107                 1833.84
00198                                 |s - d| / (|s| + |d|)^2                 67.1269                 1834.02
00199                                 log(|s - d|) / (|s| + |d|)              67.2087                 1837.67 
00200 
00201                         (s = source corner response value, d = destination corner response value)
00202 
00203                         ************************************************************
00204                         
00205                         Comando:
00206                                 ./continuous --URL=/var/www/public_data/videos/contours/big_plate.dv  --Deinterlaced=true --Cols=360 --Rows=288 --"Gauss smooth image"=true --"max worker iterations"=500
00207 
00208                                 Heuristic                               Iterations per RANSAC   Variance over iterations
00209                                 ---------------                         ---------------         ---------------
00210                                 1 / MIN(s, d)                           124.048                 105.266
00211                                 1 / MAX(s, d)                           445.706                 98.0611
00212                                 1 / (s * d)                             73.706                  68.0876
00213 
00214                         *************************************************************/
00215 
00216                         /*for (int i = 0; i < sourcePoints.size(); i++)
00217                                 for (int j = 0; j < destinationPoints.size(); j++)
00218                                         addElement(QPair<QPointF, QPointF>(sourcePoints.at(i), destinationPoints.at(j)),
00219                                                         1 / (ABS(sourceResponses.at(i)) + ABS(destinationResponses.at(j)))
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         //addProperty<int>("Maximal track points", inputFlag, 750, "Maximal points to track", 50, 5000);
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         //addProperty< QList < sFloat > >("Feature responses", inputFlag);
00258 
00259         addProperty< CulebrillaContainer >("Culebrillas container", outputFlag);
00260         }
00261 
00262 void FeatureEuclideanTracker::iterate()
00263         {
00264         // 0. Read input property values.
00265         const int       maxIterations = getPropertyValue<int>("Max iterations"),
00266                         maximalTestInliers = getPropertyValue<int>("Maximal test inliers"),
00267                         minimalTestInliers = getPropertyValue<int>("Minimal test inliers"),
00268                         //maximalTrackPoints = getPropertyValue<int>("Maximal track points"),
00269                         maxSkipFrames = getPropertyValue<int>("Skip unmatched frames");
00270         const double    maxPixelDist = getPropertyValue<double>("Max pixel dist");
00271 
00272         timeFlag("Read input properties");
00273 
00274         // 1. Get points locations and responses.
00275         //const QList<sFloat> actualResponses = getPropertyValue< QList< sFloat > >("Feature responses");
00276         const QList<QPointF> actualPoints = getPropertyValue< QList< QPointF > >("Feature locations");
00277 
00278         timeFlag("Get maximal hot points");
00279 
00280         // 2. Use RANSAC homography search to match template points and canditate points.
00281         bool matchingFound = false;
00282         //QVMatrix Hfound = QVMatrix::identity(3);
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                 //QVEuclideanRANSAC sampler(reducedLastPoints, reducedActualPoints, maxPixelDist, minimalTestInliers);
00291                 //QVEuclideanPROSAC sampler(reducedLastPoints, reducedActualPoints, lastResponses, actualResponses, maxPixelDist, minimalTestInliers);
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                         // 3.1.1. Get new actual and last point list, of size 'maximalTrackPoints'.
00300         
00301                         // - Coge el elemento de mayor respuesta de lastPoints, y busca el más cercano a este, según el valor de respuesta, que
00302                         //      supere el test de proximidad
00303                         // - Añade la correspondencia y Elimina ambos de cada lista
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                         for (int i = reducedLastPoints.size()-1; i >= 0 ; i--)
00308                                 for (int j = reducedActualPoints.size()-1; j >= 0 ; j--)
00309                                         {
00310                                         const QPointF   last = reducedLastPoints.at(i),
00311                                                         actual = reducedActualPoints.at(j);
00312                                         if (norm2(ApplyHomography(Hfound, last) - actual) < maxPixelDist)
00313                                                 {
00314                                                 finalInliers.append(QPair<QPointF, QPointF>(last, actual));
00315                                                 reducedActualPoints.removeAt(j);
00316                                                 break;                                                  
00317                                                 }
00318                                         }*/
00319         
00320                         // 3.1.2. Actualize Culebrillas with denormalized matchings.
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                 // Step culebrillas
00341                 culebrillas.step();
00342 
00343                 // Store points
00344                 lastPoints = actualPoints;
00345                 //lastResponses = actualResponses;
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         //addProperty<int>("Max iterations", inputFlag, 150, "Maximal number of iterations to perform PROSAC", 1, 1000);
00361         //addProperty<int>("Maximal test inliers", inputFlag, 40, "Number of points to perform RANSAC search per frame", 10, 100);
00362         //addProperty<int>("Minimal test inliers", inputFlag, 15, "Minimal number of matchings for a model to be considered appropiated", 3, 30);
00363         //addProperty<int>("Maximal track points", inputFlag, 750, "Maximal points to track", 50, 5000);
00364         //addProperty<int>("Skip unmatched frames", inputFlag, 10, "Number of frames with no correspondences to skip", 0, 50);
00365 
00366         addProperty< QVMatrix >("Euclidean transformation", inputFlag, QVMatrix::identity(1));
00367 
00368         addProperty< QList < QPointF > >("Feature locations", inputFlag);
00369         //addProperty< QList < sFloat > >("Feature responses", inputFlag);
00370 
00371         addProperty< CulebrillaContainer >("Culebrillas container", outputFlag);
00372         }
00373 
00374 void FeatureEuclideanMatcher::iterate()
00375         {
00376         // 0. Read input property values.
00377         const int       //maxIterations = getPropertyValue<int>("Max iterations"),
00378                         //maximalTestInliers = getPropertyValue<int>("Maximal test inliers"),
00379                         //minimalTestInliers = getPropertyValue<int>("Minimal test inliers"),
00380                         //maximalTrackPoints = getPropertyValue<int>("Maximal track points"),
00381                         //maxSkipFrames = getPropertyValue<int>("Skip unmatched frames");
00382         const double    maxPixelDist = getPropertyValue<double>("Max pixel dist");
00383 
00384         timeFlag("Read input properties");
00385 
00386         // 1. Get points locations and responses.
00387         //const QList<sFloat> actualResponses = getPropertyValue< QList< sFloat > >("Feature responses");
00388         const QList<QPointF> actualPoints = getPropertyValue< QList< QPointF > >("Feature locations");
00389 
00390         timeFlag("Get maximal hot points");
00391 
00392         // 2. Use RANSAC homography search to match template points and canditate points.
00393         bool matchingFound = false;
00394         //QVMatrix Hfound = QVMatrix::identity(3);
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                 //QVEuclideanRANSAC sampler(reducedLastPoints, reducedActualPoints, maxPixelDist, minimalTestInliers);
00403                 //QVEuclideanPROSAC sampler(reducedLastPoints, reducedActualPoints, lastResponses, actualResponses, maxPixelDist, minimalTestInliers);
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                         // 3.1.1. Get new actual and last point list, of size 'maximalTrackPoints'.
00412         
00413                         // - Coge el elemento de mayor respuesta de lastPoints, y busca el más cercano a este, según el valor de respuesta, que
00414                         //      supere el test de proximidad
00415                         // - Añade la correspondencia y Elimina ambos de cada lista
00416                         /*
00417                         const QList<QPointF> reducedLastPoints = lastPoints.mid(MAX(0, (int)lastPoints.size() - maximalTrackPoints));
00418                         QList<QPointF> reducedActualPoints = actualPoints.mid(MAX(0, (int) actualPoints.size() - maximalTrackPoints));
00419                         for (int i = reducedLastPoints.size()-1; i >= 0 ; i--)
00420                                 for (int j = reducedActualPoints.size()-1; j >= 0 ; j--)
00421                                         {
00422                                         const QPointF   last = reducedLastPoints.at(i),
00423                                                         actual = reducedActualPoints.at(j);
00424                                         if (norm2(ApplyHomography(Hfound, last) - actual) < maxPixelDist)
00425                                                 {
00426                                                 finalInliers.append(QPair<QPointF, QPointF>(last, actual));
00427                                                 reducedActualPoints.removeAt(j);
00428                                                 break;                                                  
00429                                                 }
00430                                         }*/
00431         
00432                         // 3.1.2. Actualize Culebrillas with denormalized matchings.
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                 // Step culebrillas
00453                 culebrillas.step();
00454 
00455                 // Store points
00456                 lastPoints = actualPoints;
00457                 //lastResponses = actualResponses;
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