examples/calibrate2cameras/calibrate2cameras.cpp

Go to the documentation of this file.
00001 /*
00002  *      Copyright (C) 2007. 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 
00038 #include <stdio.h>
00039 #include <stdlib.h>
00040 #include <iostream>
00041 #include <math.h>
00042 
00043 #include <QDebug>
00044 #include <QThread>
00045 
00046 #include <QVApplication>
00047 #include <QVMPlayerCamera>
00048 #include <QVGUI>
00049 #include <QVImageCanvas>
00050 #include <QVGLCanvas>
00051 
00052 #include <QVDisjointSet>
00053 #include <QV3DModel>
00054 #include <QVMatrix>
00055 #include <QVPROSAC>
00056 
00057 #include <qvmath/qvprojective.h>
00058 #include <qvdta/qvdta.h>
00059 #include <qvip/qvip.h>
00060 
00061 #ifndef DOXYGEN_IGNORE_THIS
00062 
00063 // Proyective homography PROSAC class. This is useful to get matchings between points at the input image and the template image, and
00064 // obtain the matching homography if it can be found.
00065 class QVHomographyPROSAC: public QVPROSAC< QPair<QPointF, QPointF>, QVMatrix>
00066         {
00067         private:
00068                 double maxError;
00069 
00070                 bool testCoherentMatchings(const QList< QPair<QPointF, QPointF> > &matchings) const
00071                         {
00072                         const QList<QPointF>    sourcePoints = getFirstPairList<QPointF>(matchings),
00073                                                 destinationPoints = getSecondPairList<QPointF>(matchings);
00074                 
00075                         // Check every source point is matched with only one destination point, and viceversa.
00076                         foreach (QPointF point, sourcePoints)
00077                                 if (sourcePoints.count(point) > 1)
00078                                         return false;
00079                 
00080                         foreach (QPointF point, destinationPoints)
00081                                 if (destinationPoints.count(point) > 1)
00082                                         return false;
00083                 
00084                         return true;
00085                         }
00086                 
00087                 bool testCoherentMatchings(const QList< QPair<QPointF, QPointF> > &matchings, const QPair<QPointF, QPointF> matching) const
00088                         {
00089                         QPair <QPointF, QPointF> inlierMatching;
00090                         foreach (inlierMatching, matchings)
00091                                 if (inlierMatching.first == matching.first || inlierMatching.second == matching.second)
00092                                         return false;
00093                 
00094                         return true;
00095                         }
00096 
00097         public:
00098                 QVHomographyPROSAC(     const QList<QPointF> &sourcePoints, const QList<QPointF> &destinationPoints,
00099                                         const double maxError, const QList< QPair<QPointF, QPointF> > &previousMatchings):
00100                         QVPROSAC< QPair<QPointF, QPointF>, QVMatrix>(4, 5), maxError(maxError)
00101                         {
00102                         // This iteration will respect order
00103                         foreach (QPointF source, sourcePoints)
00104                                 foreach (QPointF destination, destinationPoints)
00105                                         {
00106                                         // Look for a matching with same destination point as actual.
00107                                         double heuristic = 100000000;
00108                                         for (int i = 0; i < previousMatchings.size(); i++)
00109                                                 if (previousMatchings.at(i).second == destination)
00110                                                         heuristic = norm2(source - previousMatchings.at(i).first);
00111 
00112                                         // Add new pair to matching set.
00113                                         addElement(QPair<QPointF, QPointF>(source, destination), heuristic);
00114                                         }
00115                         init();
00116                         }
00117 
00118                 const bool fit(const QList< QPair<QPointF, QPointF> > &matchings, QVMatrix &homography)
00119                         {
00120                         if (!testCoherentMatchings(matchings))
00121                                 return false;
00122 
00123                         homography = ComputeProjectiveHomography(matchings);
00124 
00125                         return true;
00126                         };
00127 
00128                 const bool test(const QVMatrix &homography, const QPair<QPointF, QPointF> &matching)
00129                         {
00130                         if (!testCoherentMatchings(inliersSet, matching))
00131                                 return false;
00132 
00133                         return norm2(ApplyHomography(homography, matching.first) - matching.second) < maxError;
00134                         };
00135         };
00136 
00137 class TemplateCameraCalibrator: public QVWorker
00138         {
00139         private:
00140                 QList< QPair<QPointF, QPointF> > previousMatchings;
00141                 QList<QPointF>  templateFPoints;
00142 
00143                 const QList<QPointF> denormalizePoints(const QVImage<uChar> &image, const QList<QPointF> &points)
00144                         {
00145                         const double rows = image.getRows(), cols = image.getCols(), factor = cols/2;
00146                         QList<QPointF> pointFList;
00147                 
00148                         foreach(QPointF point, points)
00149                                 pointFList.append(QPointF(cols/2 + point.x()*factor, rows/2 -point.y()*factor));
00150                         return pointFList;
00151                         }
00152                 
00153                 const QList<QPointF> normalizePoints(const QVImage<uChar> &image, const QList<QPointF> &points)
00154                         {
00155                         const double rows = image.getRows(), cols = image.getCols(), factor = cols/2;
00156                         QList<QPointF> pointFList;
00157                 
00158                         foreach(QPointF point, points)
00159                                 pointFList.append(QPointF((point.x() - cols/2)/factor,-(point.y() - rows/2)/factor));
00160                         return pointFList;
00161                         }
00162 
00163         public:
00164                 TemplateCameraCalibrator(QString name, QString defaultTemplateFileName): QVWorker(name)
00165                         {
00166                         addProperty<double>("Max pixel dist", inputFlag, 0.018, "for a pixel considered to be coincident", 0.0, 0.1);
00167                         addProperty<int>("Max iterations", inputFlag, 250, "Corner response window search", 1, 5000);
00168                         addProperty<int>("Window size", inputFlag, 10, "Corner response window search", 1, 100);
00169                         addProperty<int>("Point number", inputFlag, 5, "Corner response window search", 1, 100);
00170 
00171                         addProperty< QVImage<uChar,1> >("Input image", inputFlag|outputFlag);
00172                         addProperty< QList<QPointF> >("Corners", outputFlag);
00173 
00174                         addProperty< QVMatrix >("Homography", outputFlag);
00175                         addProperty< QVMatrix >("Extrinsic matrix", outputFlag);
00176                         addProperty< QVMatrix >("Intrinsic matrix", outputFlag);
00177 
00178                         addProperty<QString>("TemplateFile", inputFlag, defaultTemplateFileName, "Path to the file containing the template");
00179 
00180                         QString templateFilePath = getPropertyValue<QString>("TemplateFile");
00181                         QVImage<uChar> templateImage;
00182                         if (QVMPlayerCamera::getFrame(templateFilePath, templateImage) )
00183                                 {
00184                                 const uInt rows = templateImage.getRows(), cols = templateImage.getCols();
00185                                 QVImage<sFloat> cornerResponseTemplateImage(cols, rows);
00186                                 SobelCornerResponseImage(templateImage, cornerResponseTemplateImage);
00187 
00188                                 QList<QPointF> pointFList = GetMaximalResponsePoints3(cornerResponseTemplateImage);
00189                                 templateFPoints = normalizePoints(cornerResponseTemplateImage, pointFList.mid(MAX(0,pointFList.size()-5)));
00190 
00191                                 if ( templateFPoints.size() == 5 )
00192                                         foreach (QPointF point, templateFPoints)
00193                                                 previousMatchings.append(QPair<QPointF, QPointF>(point, point));
00194                                 else
00195                                         setLastError("Can't get five corner points on template file");
00196                                 }
00197                         else
00198                                 setLastError(QString() + "Can't open template file '" + templateFilePath +"'.");
00199                         }
00200 
00201                 void iterate()
00202                         {
00203                         // 0. Read input property values.
00204                         const QVImage<uChar> image = getPropertyValue< QVImage<uChar,1> >("Input image");
00205                         const uInt      rows = image.getRows(), cols = image.getCols(),
00206                                         windowSize = getPropertyValue<int>("Window size"),
00207                                         maxIterations = getPropertyValue<int>("Max iterations"),
00208                                         pointNumber = getPropertyValue<int>("Point number");
00209                         const double    maxPixelDist = getPropertyValue<double>("Max pixel dist");
00210 
00211                         timeFlag("Read input properties");
00212 
00213                         // 1. Get candidate points from corner response image.
00214                         QVImage<sFloat> cornerResponseImage(cols, rows);
00215                         SobelCornerResponseImage(image, cornerResponseImage);
00216                         timeFlag("Corner response image");
00217 
00218                         QList<QPointF> maximalPoints = GetMaximalResponsePoints3(cornerResponseImage);
00219                         timeFlag("Get maximal hot points");
00220 
00221                         QList<QPointF>  imageFPoints =
00222                                 normalizePoints(image, maximalPoints.mid(MAX(0, maximalPoints.size() - pointNumber)));
00223                         timeFlag("Hot point list to vector list");
00224                         
00225                         // 2. Use RANSAC homography search to match template points and canditate points.
00226                         bool matchingFound = false;
00227                         QVMatrix Hfound = QVMatrix::identity(3);
00228 
00229                         if (maximalPoints.size() >= 5)
00230                                 {
00231                                 QVHomographyPROSAC prosac(imageFPoints, templateFPoints, maxPixelDist, previousMatchings);
00232 
00233                                 if (matchingFound = prosac.iterate(maxIterations))
00234                                         {
00235                                         Hfound = prosac.getBestModel();
00236                                         previousMatchings = prosac.getBestInliers();
00237                                         }
00238                                 }
00239 
00240                         timeFlag("RANSAC");
00241 
00242                         Hfound = Hfound / Hfound(2,2);
00243 
00244                         // 3. Store and show results.
00245                         // 3.1. Show consensus points.
00246                         setPropertyValue< QList<QPointF> >("Corners", denormalizePoints(image, getFirstPairList<QPointF>(previousMatchings)));
00247                         timeFlag("Draw consensus points");
00248 
00249                         // 3.3. Decompose homography matrix for camera intrinsic and extrinsic calibration.
00250                         if (matchingFound)
00251                                 {
00252                                 // 3.3.1 Obtain intrinsic and extrinsic camera matrix decomposition from the homography.
00253                                 QVMatrix intrinsicCameraMatrix, extrinsicCameraMatrix;
00254 
00255                                 //GetIntrinsicCameraMatrixFromHomography(H,K);
00256                                 //GetDirectIntrinsicCameraMatrixFromHomography(Hfound,intrinsicCameraMatrix);
00257                         
00258                                 // 3.3.1.2. Compute extrinsic camera matrix
00259                                 GetExtrinsicCameraMatrixFromHomography(intrinsicCameraMatrix, pseudoInverse(Hfound), extrinsicCameraMatrix);
00260 
00261                                 //DecomposeCameraMatricesFromPlanarHomography(Hfound, intrinsicCameraMatrix, extrinsicCameraMatrix);
00262 
00263                                 // 3.3.2 Store resulting matrices.
00264                                 setPropertyValue<QVMatrix>("Homography", Hfound);
00265                                 setPropertyValue<QVMatrix>("Intrinsic matrix", intrinsicCameraMatrix);
00266                                 setPropertyValue<QVMatrix>("Extrinsic matrix", extrinsicCameraMatrix);
00267                                 }
00268                         else    {
00269                                 setPropertyValue<QVMatrix>("Intrinsic matrix", QVMatrix::identity(3));
00270                                 setPropertyValue<QVMatrix>("Extrinsic matrix", QVMatrix::identity(4));
00271                                 }
00272                         timeFlag("Decompose homography matrix");
00273 
00274                         // 3.4. Show output images.
00275                         timeFlag("Draw corners");
00276                         }
00277         };
00278 
00279 class ImageHomographyWarperWorker: public QVWorker
00280         {
00281         private:
00282                 void myWarpPerspective(const QVImage<uChar> &src, QVImage<uChar> &dest, const QVMatrix &H, const double zoom)
00283                         {
00284                         const double cols = src.getCols(), rows = src.getRows();
00285                 
00286                         QVMatrix sourcePoints(cols*rows, 3);
00287                 
00288                         for (double col = 0; col < cols; col++)
00289                                 for (double row = 0; row < rows; row++)
00290                                         {
00291                                         sourcePoints(col*rows + row, 0) = 2*(col - cols/2)/cols;
00292                                         sourcePoints(col*rows + row, 1) = -2*(row - rows/2)/cols;
00293                                         sourcePoints(col*rows + row, 2) = 1;
00294                                         }
00295                 
00296                         const QVMatrix destinationPoints = (sourcePoints * H.transpose()).rowHomogeneousNormalize();
00297                 
00298                         for (double col = 0; col < cols; col++)
00299                                 for (double row = 0; row < rows; row++)
00300                                         {
00301                                         const QPoint    p2(col, row),
00302                                                         p1(     +zoom*destinationPoints(col*rows + row, 0)+cols/2,
00303                                                                 -zoom*destinationPoints(col*rows + row, 1)+rows/2);
00304                                         if (dest.getROI().contains(p2) && src.getROI().contains(p1))
00305                                                 dest(p1) = src(p2);
00306                                         }
00307                         }
00308 
00309         public:
00310                 ImageHomographyWarperWorker(QString name): QVWorker(name)
00311                         {
00312                         addProperty<double>("Zoom", inputFlag, 30, "Size of the rectified template", 1, 100);
00313                         addProperty< QVMatrix >("Homography", inputFlag);
00314 
00315                         addProperty< QVImage<uChar,1> >("Input image", inputFlag|outputFlag);
00316                         addProperty< QVImage<uChar,1> >("Wrapped image", outputFlag);
00317 
00318                         }
00319 
00320                 void iterate()
00321                         {
00322                         // 0. Read input property values.
00323                         const QVImage<uChar> image = getPropertyValue< QVImage<uChar,1> >("Input image");
00324                         const uInt      rows = image.getRows(), cols = image.getCols();
00325                         const double    zoom = getPropertyValue<double>("Zoom");
00326                         const QVMatrix  Hfound = getPropertyValue< QVMatrix>("Homography");
00327                         timeFlag("Read input properties");
00328 
00329                         // 1. Store and show results.
00330                         // 1.1. Draw wrapped image by homography.
00331                         QVImage<uChar> wrapped(cols, rows);
00332                         Set(wrapped,0);
00333                         myWarpPerspective(image, wrapped, Hfound, zoom);
00334                         setPropertyValue< QVImage<uChar,1> >("Wrapped image", wrapped);
00335                         timeFlag("Image wrapping");
00336                         }
00337         };
00338 
00339 class ImageOverlaperWorker: public QVWorker  // todo: implemetarla entera
00340         {
00341         private:
00342 
00343         public:
00344                 ImageOverlaperWorker(QString name): QVWorker(name)
00345                         {
00346                         addProperty< QVImage<uChar,1> >("Input image1", inputFlag|outputFlag);
00347                         addProperty< QVImage<uChar,1> >("Input image2", inputFlag|outputFlag);
00348                         addProperty< QVImage<uChar,1> >("Overlaped image", outputFlag);
00349                         }
00350 
00351                 void iterate()
00352                         {
00353                         // 0. .
00354                         QVImage<uChar,1> image = getPropertyValue< QVImage<uChar,1> >("Input image1");
00355 
00356                         // 1. overlap the images, using the IPP's add function.
00357                         setPropertyValue< QVImage<uChar,1> >("Overlaped image", image);
00358                         }
00359         };
00360 
00362 
00363 const QV3DModel cameraModel(const double baseSize, const double fov)
00364         {
00365         QV3DModel model;
00366 
00367         model.addSegment(-baseSize,+baseSize,0,         -baseSize,-baseSize,0,  255,255,255);
00368         model.addSegment(-baseSize,+baseSize,0,         -baseSize,+baseSize,0,  255,255,255);
00369         model.addSegment(-baseSize,+baseSize,0,         -baseSize,-baseSize,0,  255,255,255);
00370         model.addSegment(-baseSize,-baseSize,0,         -baseSize,-baseSize,0,  255,255,255);
00371         model.addSegment(+baseSize,-baseSize,0,         -baseSize,-baseSize,0,  255,255,255);
00372         model.addSegment(+baseSize,-baseSize,0,         +baseSize,-baseSize,0,  255,255,255);
00373         model.addSegment(+baseSize,+baseSize,0,         +baseSize,-baseSize,0,  255,255,255);
00374         model.addSegment(+baseSize,+baseSize,0,         -baseSize,+baseSize,0,  255,255,255);
00375         model.addSegment(+baseSize,-baseSize,0,         -baseSize,-baseSize,0,  255,255,255);
00376         model.addSegment(+baseSize,+baseSize,0,         -baseSize,+baseSize,0,  255,255,255);
00377         model.addSegment(+baseSize,+baseSize,0,         +baseSize,-baseSize,0,  255,255,255);
00378         model.addSegment(+baseSize,+baseSize,0,         +baseSize,+baseSize,0,  255,255,255);
00379 
00380         model.addSegment(0,0,0, +baseSize*1.5,0,0,      255,0,0);
00381         model.addSegment(0,0,0, 0,+baseSize*1.5,0,      0,255,0);
00382         model.addSegment(0,0,0, 0,0,+baseSize*1.5,      0,0,255);
00383 
00384         model.addSegment(0,0,+fov,      +baseSize,+baseSize,0,  192,192,192);
00385         model.addSegment(0,0,+fov,      +baseSize,-baseSize,0,  192,192,192);
00386         model.addSegment(0,0,+fov,      -baseSize,-baseSize,0,  192,192,192);
00387         model.addSegment(0,0,+fov,      -baseSize,+baseSize,0,  192,192,192);
00388 
00389         return model;
00390         }
00391 
00392 int main(int argc, char *argv[])
00393         {
00394         QVApplication app(argc, argv,
00395                 "Example program for QVision library. Obtains intrinsic and extrinsic camera parameters."
00396                 );
00397 
00398         QVMPlayerCamera camera1("Video1");
00399         QVMPlayerCamera camera2("Video2");
00400 
00401         // Camera calibrator.
00402         TemplateCameraCalibrator templateCalibrator1("Corners Worker", "template3d.gif");
00403         TemplateCameraCalibrator templateCalibrator2("Corners Worker", "template3d.gif");
00404         camera1.link(&templateCalibrator1,"Input image");
00405         camera2.link(&templateCalibrator2,"Input image");
00406 
00407         // Image warper.
00408         ImageHomographyWarperWorker warper1("Image warper");
00409         ImageHomographyWarperWorker warper2("Image warper");
00410         templateCalibrator1.linkProperty("Input image", &warper1, "Input image", QVWorker::SynchronousLink);
00411         templateCalibrator1.linkProperty("Homography", &warper1, "Homography", QVWorker::SynchronousLink);
00412         templateCalibrator2.linkProperty("Input image", &warper2, "Input image", QVWorker::SynchronousLink);
00413         templateCalibrator2.linkProperty("Homography", &warper2, "Homography", QVWorker::SynchronousLink);
00414 
00415         // Image overlap
00416         ImageOverlaperWorker overlaper("Image overlaper");
00417         warper1.linkProperty("Wrapped image", &overlaper, "Input image1", QVWorker::SynchronousLink);
00418         warper2.linkProperty("Wrapped image", &overlaper, "Input image2", QVWorker::SynchronousLink);
00419 
00420         // Image outputs
00421         QVImageCanvas imageCanvas("Corners ");
00422         imageCanvas.linkProperty(templateCalibrator1, "Input image");
00423         imageCanvas.linkProperty(templateCalibrator1,"Corners", Qt::red, true);
00424 
00425         QVImageCanvas imageCanvas2("Wrapped");
00426         imageCanvas2.linkProperty(warper1, "Wrapped image");
00427 
00428         QVImageCanvas imageCanvas3("Overlaped");
00429         imageCanvas3.linkProperty(overlaper, "Overlaped image");
00430 
00431         // 3D representation of cameras.  todo: incluir la segunda camara en el esquema
00432         QVGLCanvas glCanvas("3d camera location");
00433         glCanvas.add(QV3DModel::referenceCoordinates(0.75, false), "Coordinate axis");
00434         glCanvas.add(QV3DModel::grid(0.25, 0.25, 15, 15), "Floor grid");
00435 
00436         QVImage<uChar,1> templateImage;
00437         if (QVMPlayerCamera::getFrame("template3d.gif", templateImage))
00438                 glCanvas.add(QV3DModel::image(templateImage), "Template image");
00439         glCanvas.add(cameraModel(0.25, 0.5), "Camera");
00440         glCanvas.linkModelMatrix(templateCalibrator1, "Extrinsic matrix" , "Camera");
00441 
00442         /*
00443         #define Camera(N)                                                                                       \
00444                 QVMPlayerCamera camera ## N("Video " # N);                                                      \
00445                 TemplateCameraCalibrator templateCalibrator ## N("Corners Worker " # N, "template3d.gif");      \
00446                 camera ## N.link(&templateCalibrator ## N,"Input image");                                       \
00447                                                                                                                 \
00448                 QVImageCanvas imageCanvas ## N("Corners " # N);                                                 \
00449                 imageCanvas ## N.linkProperty(templateCalibrator ## N, "Input image");                          \
00450                 imageCanvas ## N.linkProperty(templateCalibrator ## N,"Corners", Qt::red);                      \
00451                                                                                                                 \
00452                 glCanvas.add(cameraModel(0.25, 0.5), "Camera" # N);                                             \
00453                 glCanvas.linkModelMatrix(templateCalibrator ## N, "Extrinsic matrix" , "Camera" # N);           \
00454 
00455         Camera(1);
00456         Camera(2);
00457         Camera(3);
00458         */
00459 
00460         QVGUI interface;
00461 
00462         return app.exec();
00463         }
00464 
00465 #endif
00466 
00467 

Generated on Thu Jul 17 17:23:27 2008 for QVision by  doxygen 1.5.3