examples/realtimePlanarRectificator/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 bool testCoherentMatchings(const QList< QPair<QPointF, QPointF> > &matchings)
00031         {
00032         const QList<QPointF>    sourcePoints = getFirstPairList<QPointF>(matchings),
00033                                 destinationPoints = getSecondPairList<QPointF>(matchings);
00034 
00035         // Check every source point is matched with only one destination point, and viceversa.
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                         //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 
00111                         //init();
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                         Times and iterations for file 'http://perception.inf.um.es/public_data/videos/templates/plantillas-MSER/plantilla.avi'
00177                         For first 1000 input frames:
00178 
00179                                 Heuristic                               RANSAC time             Iterations per RANSAC
00180                                 ---------------                         ---------------         ---------------
00181                                 0 (sorted for corner response value)    8.92173                 625.286
00182                                 rand()                                  90.1418                 2506.99
00183                                 1 / (|s| + |d|)                         4.46567                 97.2883
00184                                 1 / (log(|s|) + log(|d|))               4.55425                 97.2893
00185                                 1 / (|s| * |d|)                         4.34896                 97.2492
00186                                 1 / |s - d|                             161.865                 4900.74
00187                                 |s - d|                                 67.087                  1831.34
00188                                 |s - d| / (|s| + |d|)                   66.7107                 1833.84
00189                                 |s - d| / (|s| + |d|)^2                 67.1269                 1834.02
00190                                 log(|s - d|) / (|s| + |d|)              67.2087                 1837.67 
00191 
00192                         (s = source corner response value, d = destination corner response value)
00193 
00194                         ************************************************************
00195                         
00196                         Comando:
00197                                 ./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
00198 
00199                                 Heuristic                               Iterations per RANSAC   Variance over iterations
00200                                 ---------------                         ---------------         ---------------
00201                                 1 / MIN(s, d)                           124.048                 105.266
00202                                 1 / MAX(s, d)                           445.706                 98.0611
00203                                 1 / (s * d)                             73.706                  68.0876
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                         //init();
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         // 0. Read input property values.
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         // 1. Get points locations and responses.
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         // 2. Use RANSAC homography search to match template points and canditate points.
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                 //QVEuclideanRANSAC sampler(reducedLastPoints, reducedActualPoints, maxPixelDist, minimalTestInliers);
00282                 //QVEuclideanPROSAC sampler(reducedLastPoints, reducedActualPoints, lastResponses, actualResponses, maxPixelDist, minimalTestInliers);
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         // 3. Store and show results.
00301         // 3.1. Show Culebrillas
00302         if (matchingFound)
00303                 {
00304                 // 3.1.1. Get new actual and last point list, of size 'maximalTrackPoints'.
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                 // - Coge el elemento de mayor respuesta de lastPoints, y busca el más cercano a este, según el valor de respuesta, que
00309                 //      supere el test de proximidad
00310                 // - Añade la correspondencia y Elimina ambos de cada lista
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                 // 3.1.2. Actualize Culebrillas with denormalized matchings.
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                 //std::cout << "ERROR: NO MATCHING FOUND at iteration " << getIteration() << std::endl;
00333                 ignoredFrames++;
00334                 }
00335 
00336         if (matchingFound || ignoredFrames >= maxSkipFrames)
00337                 {
00338                 // Step culebrillas
00339                 culebrillas.step();
00340 
00341                 // Store points
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