00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
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
00058 if (norm2(matching.first - matching.second) < minPixelDist)
00059 return false;
00060
00061 matchings.append(matching);
00062 }
00063
00064
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
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
00099
00100
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
00127 setMinimumDelay(getPropertyValue<int>("Minimum delay"));
00128
00129
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
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
00150 planarCulebrillas.intersect(longCulebrillas);
00151
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
00161
00162
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