00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
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
00064
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
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
00103 foreach (QPointF source, sourcePoints)
00104 foreach (QPointF destination, destinationPoints)
00105 {
00106
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
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
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
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
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
00245
00246 setPropertyValue< QList<QPointF> >("Corners", denormalizePoints(image, getFirstPairList<QPointF>(previousMatchings)));
00247 timeFlag("Draw consensus points");
00248
00249
00250 if (matchingFound)
00251 {
00252
00253 QVMatrix intrinsicCameraMatrix, extrinsicCameraMatrix;
00254
00255
00256
00257
00258
00259 GetExtrinsicCameraMatrixFromHomography(intrinsicCameraMatrix, pseudoInverse(Hfound), extrinsicCameraMatrix);
00260
00261
00262
00263
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
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
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
00330
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
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
00354 QVImage<uChar,1> image = getPropertyValue< QVImage<uChar,1> >("Input image1");
00355
00356
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
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
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
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
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
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
00444
00445
00446
00447
00448
00449
00450
00451
00452
00453
00454
00455
00456
00457
00458
00459
00460 QVGUI interface;
00461
00462 return app.exec();
00463 }
00464
00465 #endif
00466
00467