examples/cameraCalibrator/planartemplatedetector.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 <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 // Proyective homography PROSAC class. This is useful to get matchings between points at the input image and the template image, and
00038 // obtain the matching homography if it can be found.
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                         // Check every source point is matched with only one destination point, and viceversa.
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                         // This iteration will respect order
00077                         foreach (QPointF source, sourcePoints)
00078                                 foreach (QPointF destination, destinationPoints)
00079                                         {
00080                                         // Look for a matching with same destination point as actual.
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                                         // Add new pair to matching set.
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         // 0. Read input property values.
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         // 1. Get candidate points from corner response image.
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         // 2. Use RANSAC homography search to match template points and canditate points.
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         // 3. Store and show results.
00210         // 3.1. Show consensus points.
00211         setPropertyValue< QList<QPointF> >("Corners", denormalizePoints(image, getFirstPairList<QPointF>(previousMatchings)));
00212         timeFlag("Draw consensus points");
00213 
00214         // 3.3. Decompose homography matrix for camera intrinsic and extrinsic calibration.
00215         if (matchingFound)
00216                 {
00217                 // 3.3.1 Obtain intrinsic and extrinsic camera matrix decomposition from the homography.
00218 
00219                 //QVMatrix intrinsicCameraMatrix, extrinsicCameraMatrix;
00220                 //DecomposeCameraMatricesFromPlanarHomography(Hfound, intrinsicCameraMatrix, extrinsicCameraMatrix);
00221 
00222                 // 3.3.2 Store resulting matrices.
00223                 setPropertyValue<QVMatrix>("Homography", Hfound);
00224                 }
00225         else
00226                 setPropertyValue<QVMatrix>("Homography", QVMatrix());
00227 
00228         timeFlag("Decompose homography matrix");
00229 
00230         // 3.4. Show output images.
00231         timeFlag("Draw corners");
00232         }
00233 
00234 #endif
00235 
00236