00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00024
00025 #include <stdio.h>
00026 #include <stdlib.h>
00027 #include <iostream>
00028 #include <fstream>
00029 #include <math.h>
00030
00031 #include <QVRANSAC>
00032 #include <QVMPlayerCamera>
00033
00034 #include <planartemplatedetector.h>
00035
00036 #ifndef DOXYGEN_IGNORE_THIS
00037
00038
00039 class QVHomographyPROSAC: public QVPROSAC< QPair<QPointF, QPointF>, QVMatrix>
00040 {
00041 private:
00042 double maxError;
00043
00044 bool testCoherentMatchings(const QList< QPair<QPointF, QPointF> > &matchings) const
00045 {
00046 const QList<QPointF> sourcePoints = getFirstPairList<QPointF>(matchings),
00047 destinationPoints = getSecondPairList<QPointF>(matchings);
00048
00049
00050 foreach (QPointF point, sourcePoints)
00051 if (sourcePoints.count(point) > 1)
00052 return false;
00053
00054 foreach (QPointF point, destinationPoints)
00055 if (destinationPoints.count(point) > 1)
00056 return false;
00057
00058 return true;
00059 }
00060
00061 bool testCoherentMatchings(const QList< QPair<QPointF, QPointF> > &matchings, const QPair<QPointF, QPointF> matching) const
00062 {
00063 QPair <QPointF, QPointF> inlierMatching;
00064 foreach (inlierMatching, matchings)
00065 if (inlierMatching.first == matching.first || inlierMatching.second == matching.second)
00066 return false;
00067
00068 return true;
00069 }
00070
00071 public:
00072 QVHomographyPROSAC( const QList<QPointF> &sourcePoints, const QList<QPointF> &destinationPoints,
00073 const double maxError, const QList< QPair<QPointF, QPointF> > &previousMatchings):
00074 QVPROSAC< QPair<QPointF, QPointF>, QVMatrix>(4, 5), maxError(maxError)
00075 {
00076
00077 foreach (QPointF source, sourcePoints)
00078 foreach (QPointF destination, destinationPoints)
00079 {
00080
00081 double heuristic = 100000000;
00082 for (int i = 0; i < previousMatchings.size(); i++)
00083 if (previousMatchings.at(i).second == destination)
00084 heuristic = norm2(source - previousMatchings.at(i).first);
00085
00086
00087 addElement(QPair<QPointF, QPointF>(source, destination), heuristic);
00088 }
00089 init();
00090 }
00091
00092 const bool fit(const QList< QPair<QPointF, QPointF> > &matchings, QVMatrix &homography)
00093 {
00094 if (!testCoherentMatchings(matchings))
00095 return false;
00096
00097 homography = ComputeProjectiveHomography(matchings);
00098
00099 return true;
00100 };
00101
00102 const bool test(const QVMatrix &homography, const QPair<QPointF, QPointF> &matching)
00103 {
00104 if (!testCoherentMatchings(inliersSet, matching))
00105 return false;
00106
00107 return norm2(ApplyHomography(homography, matching.first) - matching.second) < maxError;
00108 };
00109 };
00110
00111 const QList<QPointF> PlanarTemplateDetector::denormalizePoints(const QVImage<uChar> &image, const QList<QPointF> &points)
00112 {
00113 const double rows = image.getRows(), cols = image.getCols(), factor = cols/2;
00114 QList<QPointF> pointFList;
00115
00116 foreach(QPointF point, points)
00117 pointFList.append(QPointF(cols/2 + point.x()*factor, rows/2 -point.y()*factor));
00118 return pointFList;
00119 }
00120
00121 const QList<QPointF> PlanarTemplateDetector::normalizePoints(const QVImage<uChar> &image, const QList<QPointF> &points)
00122 {
00123 const double rows = image.getRows(), cols = image.getCols(), factor = cols/2;
00124 QList<QPointF> pointFList;
00125
00126 foreach(QPointF point, points)
00127 pointFList.append(QPointF((point.x() - cols/2)/factor,-(point.y() - rows/2)/factor));
00128 return pointFList;
00129 }
00130
00131 PlanarTemplateDetector::PlanarTemplateDetector(QString name, QString defaultTemplateFileName): QVWorker(name)
00132 {
00133 addProperty<double>("Max pixel dist", inputFlag, 0.018, "for a pixel considered to be coincident", 0.0, 0.1);
00134 addProperty<int>("Max iterations", inputFlag, 250, "Corner response window search", 1, 5000);
00135 addProperty<int>("Window size", inputFlag, 10, "Corner response window search", 1, 100);
00136 addProperty<int>("Point number", inputFlag, 5, "Corner response window search", 1, 100);
00137
00138 addProperty< QVImage<uChar,3> >("Input image", inputFlag|outputFlag);
00139 addProperty< QList<QPointF> >("Corners", outputFlag);
00140
00141 addProperty< QVMatrix >("Homography", outputFlag);
00142
00143 addProperty<QString>("TemplateFile", inputFlag, defaultTemplateFileName, "Path to the file containing the template");
00144
00145 QString templateFilePath = getPropertyValue<QString>("TemplateFile");
00146 QVImage<uChar> templateImage;
00147 if (QVMPlayerCamera::getFrame(templateFilePath, templateImage) )
00148 {
00149 const uInt rows = templateImage.getRows(), cols = templateImage.getCols();
00150 QVImage<sFloat> cornerResponseTemplateImage(cols, rows);
00151 FilterHessianCornerResponseImage(templateImage, cornerResponseTemplateImage);
00152
00153 QList<QPointF> pointFList = GetMaximalResponsePoints3(cornerResponseTemplateImage);
00154 templateFPoints = normalizePoints(cornerResponseTemplateImage, pointFList.mid(MAX(0,pointFList.size()-5)));
00155
00156 if ( templateFPoints.size() == 5 )
00157 foreach (QPointF point, templateFPoints)
00158 previousMatchings.append(QPair<QPointF, QPointF>(point, point));
00159 else
00160 setLastError("Can't get five corner points on template file");
00161 }
00162 else
00163 setLastError(QString() + "Can't open template file '" + templateFilePath +"'.");
00164 }
00165
00166 void PlanarTemplateDetector::iterate()
00167 {
00168
00169 const QVImage<uChar> image = getPropertyValue< QVImage<uChar,3> >("Input image");
00170 const uInt rows = image.getRows(), cols = image.getCols(),
00171 windowSize = getPropertyValue<int>("Window size"),
00172 maxIterations = getPropertyValue<int>("Max iterations"),
00173 pointNumber = getPropertyValue<int>("Point number");
00174 const double maxPixelDist = getPropertyValue<double>("Max pixel dist");
00175
00176 timeFlag("Read input properties");
00177
00178
00179 QVImage<sFloat> cornerResponseImage(cols, rows);
00180 FilterHessianCornerResponseImage(image, cornerResponseImage);
00181 timeFlag("Corner response image");
00182
00183 QList<QPointF> maximalPoints = GetMaximalResponsePoints3(cornerResponseImage);
00184 timeFlag("Get maximal hot points");
00185
00186 QList<QPointF> imageFPoints =
00187 normalizePoints(image, maximalPoints.mid(MAX(0, maximalPoints.size() - pointNumber)));
00188 timeFlag("Hot point list to vector list");
00189
00190
00191 bool matchingFound = false;
00192 QVMatrix Hfound = QVMatrix::identity(3);
00193
00194 if (maximalPoints.size() >= 5)
00195 {
00196 QVHomographyPROSAC prosac(imageFPoints, templateFPoints, maxPixelDist, previousMatchings);
00197
00198 if (matchingFound = prosac.iterate(maxIterations))
00199 {
00200 Hfound = prosac.getBestModel();
00201 previousMatchings = prosac.getBestInliers();
00202 }
00203 }
00204
00205 timeFlag("RANSAC");
00206
00207 Hfound = Hfound / Hfound(2,2);
00208
00209
00210
00211 setPropertyValue< QList<QPointF> >("Corners", denormalizePoints(image, getFirstPairList<QPointF>(previousMatchings)));
00212 timeFlag("Draw consensus points");
00213
00214
00215 if (matchingFound)
00216 {
00217
00218
00219
00220
00221
00222
00223 setPropertyValue<QVMatrix>("Homography", Hfound);
00224 }
00225 else
00226 setPropertyValue<QVMatrix>("Homography", QVMatrix());
00227
00228 timeFlag("Decompose homography matrix");
00229
00230
00231 timeFlag("Draw corners");
00232 }
00233
00234 #endif
00235
00236