examples/realtimePlanarRectificator/planardetector.cpp

Go to the documentation of this file.
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 <QVPROSAC>
00026 #include <planardetector.h>
00027 
00028 #ifndef DOXYGEN_IGNORE_THIS
00029 
00030 class QVProjectiveRANSAC2: public QVRANSAC< Culebrilla, QSet<Culebrilla> >
00031         {
00032         private:
00033                 double maxPixelDist, minPixelDist;
00034                 int lastFrame;
00035         public:
00036                 QVProjectiveRANSAC2(const QSet<Culebrilla> &culebrillas, const int lastFrame,
00037                         const double maxPixelDist, const double minPixelDist, const int minTestInliers):
00038                                 QVRANSAC< Culebrilla, QSet<Culebrilla> >(6,minTestInliers),
00039                                 maxPixelDist(maxPixelDist), minPixelDist(minPixelDist),
00040                                 lastFrame(lastFrame)
00041                         {
00042                         foreach(Culebrilla culebrilla, culebrillas)
00043                                 addElement(culebrilla);
00044                         }
00045 
00046                 const bool fit(const QList< Culebrilla > &culebrillas, QSet<Culebrilla> & culebrillaSet)
00047                         {
00048                         int firstFrame = 0;
00049                         foreach(Culebrilla culebrilla, culebrillas)
00050                                 firstFrame = MAX(firstFrame, culebrilla.getFirstFrameNumber());
00051 
00052                         QList< QPair<QPointF, QPointF> > matchings;
00053                         foreach(Culebrilla culebrilla, culebrillas)
00054                                 {
00055                                 QPair<QPointF, QPointF> matching(culebrilla.getPointAtFrame(firstFrame), culebrilla.getPointAtFrame(lastFrame));
00056 
00057                                 // If the points are too close, culebrilla doesn't match
00058                                 if (norm2(matching.first - matching.second) < minPixelDist)
00059                                         return false;
00060 
00061                                 matchings.append(matching);
00062                                 }
00063 
00064                         // We won't keep matchings of close points
00065                         for (int i = 0; i < matchings.size()-1; i++)
00066                                 for (int j = i+1; j < matchings.size(); j++)
00067                                         if (    (norm2(matchings.at(i).first - matchings.at(j).first) < minPixelDist)   ||
00068                                                 (norm2(matchings.at(i).second - matchings.at(j).second) < minPixelDist) )
00069                                                 return false;
00070 
00071                         const QVMatrix homography = ComputeProjectiveHomography(matchings);
00072 
00073                         QPair<QPointF, QPointF> matching;
00074                         foreach(matching, matchings)
00075                                 if (norm2(ApplyHomography(homography, matching.first) - matching.second) > maxPixelDist)
00076                                         return false;
00077 
00078                         culebrillaSet = QSet<Culebrilla>::fromList(culebrillas);
00079 
00080                         return true;
00081                         };
00082 
00083                 const bool test(const QSet<Culebrilla> & culebrillaSet, const Culebrilla &culebrilla)
00084                         {
00085                         int firstFrame = culebrilla.getFirstFrameNumber();
00086                         foreach(Culebrilla matchingCulebrilla, culebrillaSet)
00087                                 firstFrame = MAX(firstFrame, matchingCulebrilla.getFirstFrameNumber());
00088 
00089                         const QPair<QPointF, QPointF> matching(culebrilla.getPointAtFrame(firstFrame), culebrilla.getPointAtFrame(lastFrame));
00090 
00091                         // Get homography for culebrilla
00092                         QList< QPair<QPointF, QPointF> > matchings;
00093                         foreach(Culebrilla matchingCulebrilla, culebrillaSet)
00094                                 {
00095                                 QPair<QPointF, QPointF> matching(       matchingCulebrilla.getPointAtFrame(firstFrame),
00096                                                                         matchingCulebrilla.getPointAtFrame(lastFrame));
00097 
00098                                 // If the points are too close, culebrilla doesn't match
00099                                 //if (norm2(matching.first - matching.second) < minPixelDist)
00100                                 //      return false;
00101                                 matchings.append(matching);
00102                                 }
00103 
00104                         const QVMatrix homography = ComputeProjectiveHomography(matchings);
00105 
00106                         return norm2(ApplyHomography(homography, matching.first) - matching.second) < maxPixelDist;
00107                         };
00108         };
00109 
00110 
00111 PlanarDetector::PlanarDetector(QString name, const bool performPlanarDetection): QVWorker(name),
00112         planarCulebrillas(), performPlanarDetection(performPlanarDetection)
00113         {
00114         addProperty< int >("Minimum delay", inputFlag, 500, "", 0, 4000);
00115         addProperty< int >("Max number of tries", inputFlag, 400, "", 0, 10000);
00116         addProperty< int >("Minimum culebrillas for plane", inputFlag, 8, "", 5, 55);
00117         addProperty< int >("Minimum culebrilla size", inputFlag|outputFlag, 20, "", 1, 200);
00118         addProperty< double >("Max point dist", inputFlag, 0.01, "for a pixel considered to be coincident", 0.0, 0.1);
00119 
00120         addProperty< CulebrillaContainer >("Culebrillas container", inputFlag|outputFlag);
00121         addProperty< QSet<Culebrilla> >("Planar culebrillas", outputFlag);
00122         }
00123 
00124 void PlanarDetector::iterate()
00125         {
00126         // 0. Actualize iterate execution delay.
00127         setMinimumDelay(getPropertyValue<int>("Minimum delay"));
00128 
00129         // 1. Get culebrillas.
00130         const CulebrillaContainer culebrillaContainer = getPropertyValue< CulebrillaContainer >("Culebrillas container");
00131         const int       numberCulebrillasNeeded = getPropertyValue<int>("Minimum culebrillas for plane"),
00132                         minimumCulebrillaSize = getPropertyValue<int>("Minimum culebrilla size"),
00133                         maxNumberOfTries = getPropertyValue<int>("Max number of tries"),
00134                         actualFrame = culebrillaContainer.getActualFrameNumber();
00135         const double    maxPointDist = getPropertyValue<double>("Max point dist");
00136 
00137         // Get actual culebrillas long enough
00138         QSet < Culebrilla > longCulebrillas;
00139         foreach(Culebrilla culebrilla, culebrillaContainer.getCulebrillas())
00140                 if (culebrilla.getNumberOfFrames() > minimumCulebrillaSize)
00141                         longCulebrillas.insert(culebrilla);
00142 
00143         if (!performPlanarDetection)
00144                 {
00145                 setPropertyValue< QSet< Culebrilla > >("Planar culebrillas", longCulebrillas);
00146                 return;
00147                 }
00148 
00149         // If already have enough culebrillas, match for new ones and stop.
00150         planarCulebrillas.intersect(longCulebrillas);
00151         //QVProjectiveRANSAC sampler(longCulebrillas, culebrillaContainer.getActualFrameNumber(), maxPointDist, 0.05, numberCulebrillasNeeded);
00152         QVProjectiveRANSAC2 sampler(longCulebrillas, culebrillaContainer.getActualFrameNumber(), maxPointDist, 0.025, numberCulebrillasNeeded);
00153         const bool matchingFound = sampler.iterate(maxNumberOfTries);
00154         QSet<Culebrilla> newPlanarCulebrillas = QSet<Culebrilla>::fromList(sampler.getBestInliers());
00155 
00156         if (planarCulebrillas.size() >= numberCulebrillasNeeded)
00157                 {
00158                 /*
00159 
00160                 foreach(Culebrilla culebrilla, longCulebrillas)
00161                         if (sampler.test(planarCulebrillas, culebrilla))
00162                                 planarCulebrillas.insert(culebrilla);*/
00163                 QSet<Culebrilla> tempPlanar = planarCulebrillas;
00164                 if (planarCulebrillas.intersect(newPlanarCulebrillas).size() >= numberCulebrillasNeeded)
00165                         {
00166                         planarCulebrillas |= newPlanarCulebrillas;
00167                         std::cout << "NNNNNNNNNNNNNNEEEEEEEWWWWWWWWW Previous culebrillas found " << planarCulebrillas.size() << std::endl;
00168                         setPropertyValue< QSet< Culebrilla > >("Planar culebrillas", planarCulebrillas);
00169                         }
00170                 }
00171         else
00172                 {
00173                 planarCulebrillas.clear();
00174 
00175                 if (matchingFound)
00176                         {
00177                         std::cout << "Found planar culebrillas" << std::endl;
00178                         planarCulebrillas = newPlanarCulebrillas;
00179                         }
00180                 else
00181                         std::cout << "NOT Found planar culebrillas" << std::endl;
00182 
00183                 setPropertyValue< QSet< Culebrilla > >("Planar culebrillas", planarCulebrillas);
00184                 }
00185         }
00186 
00187 #endif