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
00042 #include <QVApplication>
00043 #include <QVMPlayerCamera>
00044 #include <QVGUI>
00045 #include <QVImageCanvas>
00046
00047 #include <QVMatrix>
00048 #include <QVPlanarRectifierWorker>
00049 #include <qvmath/qvprojective.h>
00050
00051 #include <hessiancornersextractor.h>
00052 #include <featuretracker.h>
00053
00054 #ifndef DOXYGEN_IGNORE_THIS
00055
00057
00058 #define FOCAL_LOGITEC_QUICKCAM_PRO_9000 1.7338
00059 #define FOCAL_SONY_DV_MAX_ZOOM_360_288 2.9077
00060 #define FOCAL FOCAL_SONY_DV_MAX_ZOOM_360_288
00061
00062 #include <QVMultidimensionalMinimizer>
00063 class RectifierMinimizator: public QVMultidimensionalMinimizer
00064 {
00065 private:
00066 const QVMatrix K;
00067 QList<QVMatrix> matrices;
00068
00069 const QVMatrix cameraMatrix(const double focal) const
00070 {
00071 QVMatrix K = QVMatrix::identity(3);
00072 K(2,2) = 1/focal;
00073
00074 return K;
00075 }
00076
00077 const QVMatrix getSonyDVMaxZoom360x288CameraMatrix() const
00078 {
00079 QVMatrix K(3,3);
00080
00081 K(0,0) = +2.691209; K(0,1) = -0.047614; K(0,2) = +0.095754;
00082 K(1,0) = +0.000000; K(1,1) = +2.925366; K(1,2) = -0.151462;
00083 K(2,0) = +0.000000; K(2,1) = +0.000000; K(2,2) = +1.000000;
00084
00085 return K;
00086 }
00087
00088 public:
00089 RectifierMinimizator(const QList<QVMatrix> &homographyList): QVMultidimensionalMinimizer(2, 1e-8),
00090 K(getSonyDVMaxZoom360x288CameraMatrix())
00091 {
00092 foreach(const QVMatrix H, homographyList)
00093 matrices.append(K.transpose() * H.transpose() / (K * K.transpose()) * H * K);
00094 }
00095
00096 double function(const QVVector &v)
00097 {
00098 const QVMatrix M_pi = rotationMatrix(v[0], v[1]);
00099 const QVMatrix M_pi_transpose = M_pi.transpose();
00100
00101 double accum = 0;
00102 foreach (QVMatrix mat, matrices)
00103 {
00104
00105 const QVMatrix error = M_pi_transpose * mat * M_pi;
00106 accum += POW2(error(0,1)/error(0,0)) + POW2(error(1,1)/error(0,0) - 1);
00107 }
00108
00109 return accum;
00110 }
00111
00112 const QVMatrix getHomography(const QVVector &v) const { return K * rotationMatrix(v[0], v[1]) / K; }
00113
00114 const QVMatrix rotationMatrix(const double roll, const double tilt) const
00115 {
00116
00117
00118
00119
00120
00121 QVMatrix result(3,3);
00122 result(0,0) = +cos(roll); result(0,1) = -cos(tilt)*sin(roll); result(0,2) = -sin(tilt)*sin(roll);
00123 result(1,0) = +sin(roll); result(1,1) = +cos(tilt)*cos(roll); result(1,2) = +sin(tilt)*cos(roll);
00124 result(2,0) = +0; result(2,1) = -sin(tilt); result(2,2) = +cos(tilt);
00125
00126 return result;
00127 }
00128 };
00129
00131
00132 class PlanarDetector: public QVWorker
00133 {
00134 public:
00135 PlanarDetector(QString name): QVWorker(name)
00136 {
00137 addProperty< CulebrillaContainer >("Culebrillas container", inputFlag);
00138 addProperty< int >("Minimum delay", inputFlag, 500, "", 0, 4000);
00139 addProperty< int >("Minimum culebrilla size", inputFlag, 100, "", 1, 200);
00140
00141
00142 addProperty< int >("Frame for planar rectification homography", inputFlag|outputFlag);
00143 addProperty< QSet<Culebrilla> >("Planar culebrillas", outputFlag);
00144 }
00145
00146 void iterate()
00147 {
00148
00149 setMinimumDelay(getPropertyValue<int>("Minimum delay"));
00150
00151
00152 const CulebrillaContainer culebrillaContainer = getPropertyValue< CulebrillaContainer >("Culebrillas container");
00153 const int minimumCulebrillaSize = getPropertyValue<int>("Minimum culebrilla size"),
00154 actualFrame = culebrillaContainer.getActualFrameNumber();
00155
00156 const QList < Culebrilla > culebrillas = culebrillaContainer.getCulebrillas();
00157
00158
00159 QSet<Culebrilla> planarCulebrillas;
00160
00161 foreach(Culebrilla culebrilla, culebrillas)
00162 if (culebrilla.getFirstFrameNumber() < actualFrame - minimumCulebrillaSize)
00163 planarCulebrillas.insert(culebrilla);
00164
00165 setPropertyValue< QSet< Culebrilla > >("Planar culebrillas", planarCulebrillas);
00166 setPropertyValue<int>("Frame for planar rectification homography", culebrillaContainer.getActualFrameNumber());
00167 }
00168 };
00169
00170
00171 class PlanarCharacterizator: public QVWorker
00172 {
00173 private:
00174 QSet<Culebrilla> previousPlanarCulebrillas;
00175
00176 public:
00177 PlanarCharacterizator(QString name): QVWorker(name), previousPlanarCulebrillas()
00178 {
00179 addProperty< int >("Minimum culebrillas for plane", inputFlag, 12, "", 5, 55);
00180
00181
00182 addProperty< QSet<Culebrilla> >("Planar culebrillas", inputFlag|outputFlag);
00183
00184
00185 addProperty< int >("Frame for planar rectification homography", inputFlag|outputFlag);
00186 addProperty< QVMatrix >("Planar rectification homography", outputFlag);
00187 }
00188
00189 void iterate()
00190 {
00191 const int lastFrame = getPropertyValue<int>("Frame for planar rectification homography");
00192 const int numberCulebrillasNeeded = getPropertyValue<int>("Minimum culebrillas for plane");
00193 const QSet<Culebrilla> planarCulebrillas = getPropertyValue< QSet<Culebrilla> >("Planar culebrillas");
00194
00195
00196 if ( (previousPlanarCulebrillas.intersect(planarCulebrillas).size() < numberCulebrillasNeeded) &&
00197 planarCulebrillas.size() >= numberCulebrillasNeeded)
00198 {
00199 std::cout << "**************** Recalculating rectification." << std::endl;
00200
00201 int firstFrame = 0;
00202 foreach(Culebrilla culebrilla, planarCulebrillas)
00203 firstFrame = MAX(firstFrame, culebrilla.getFirstFrameNumber());
00204
00205 if (firstFrame >= lastFrame)
00206 std::cout << "ERROR!!!!!!!!!!!!!!" << std::endl;
00207
00208
00209 QList<QVMatrix> homographies;
00210
00211 for (int index = MAX(firstFrame, lastFrame - 100); index < lastFrame; index+=4)
00212 {
00213 QList< QPair<QPointF, QPointF> > matchings;
00214 foreach(Culebrilla culebrilla, planarCulebrillas)
00215 matchings.append(
00216 QPair<QPointF, QPointF>(culebrilla.getPointAtFrame(lastFrame), culebrilla.getPointAtFrame(index))
00217 );
00218 homographies.append(ComputeProjectiveHomography(matchings));
00219 }
00220
00221
00222 RectifierMinimizator minimizator(homographies);
00223 const QList<QVVector> minimums = minimizator.gridIterate(QVVector(0,0), QVVector(PI,PI), 5, 300, 1e-10);
00224
00225 if (minimums.size() > 0)
00226 {
00227 setPropertyValue< QVMatrix >("Planar rectification homography", minimizator.getHomography(minimizator.getMinimum()));
00228
00229 previousPlanarCulebrillas = planarCulebrillas;
00230 }
00231 }
00232 }
00233 };
00234
00236
00237 class PlanarTracker: public QVWorker
00238 {
00239 private:
00240 QMap<int, QVMatrix> previousH;
00241
00242 public:
00243 PlanarTracker(QString name): QVWorker(name)
00244 {
00245 addProperty< QSet<Culebrilla> >("Planar culebrillas", inputFlag);
00246 addProperty< int >("Frame for planar rectification homography", inputFlag);
00247 addProperty< QVMatrix >("Planar rectification homography", inputFlag);
00248
00249 addProperty< CulebrillaContainer >("Culebrillas container", inputFlag);
00250
00251 addProperty< QVMatrix >("Homography", outputFlag);
00252 }
00253
00254 void iterate()
00255 {
00256 setPropertyValue<QVMatrix>("Homography", QVMatrix::identity(3));
00257
00258
00259 const CulebrillaContainer culebrillaContainer = getPropertyValue< CulebrillaContainer >("Culebrillas container");
00260 const QSet<Culebrilla> planarCulebrillas = getPropertyValue< QSet<Culebrilla> >("Planar culebrillas");
00261
00262
00263 QSet<Culebrilla> culebrillas;
00264 foreach(Culebrilla culebrilla, culebrillaContainer.getCulebrillas())
00265 if (planarCulebrillas.contains(culebrilla))
00266 culebrillas.insert(culebrilla);
00267
00268
00269 if (culebrillas.size() < 8)
00270 {
00271 previousH.clear();
00272 setPropertyValue<QVMatrix>("Homography", QVMatrix::identity(3));
00273 return;
00274 }
00275
00276
00277 int firstFrame = 0, lastFrame = culebrillaContainer.getActualFrameNumber();
00278 foreach(Culebrilla culebrilla, culebrillas)
00279 firstFrame = MAX(firstFrame, culebrilla.getFirstFrameNumber());
00280
00281
00282
00283 if (previousH.size() == 0)
00284 {
00285 const int rectificationFrame = getPropertyValue< int >("Frame for planar rectification homography");
00286 const QVMatrix rectificationHomography = getPropertyValue< QVMatrix >("Planar rectification homography");
00287
00288 previousH.clear();
00289 previousH[rectificationFrame] = rectificationHomography;
00290 std::cout << "Got homography for frame " << rectificationFrame
00291 << "; [firstFrame, lastFrame] = [" << firstFrame << ", " << lastFrame << "]" << std::endl;
00292 }
00293
00294
00295 QList< QPair<QPointF, QPointF> > matchings;
00296
00297 for (int index = firstFrame; ; index++)
00298 if (previousH.contains(index))
00299 {
00300 foreach(Culebrilla culebrilla, culebrillas)
00301 matchings.append(
00302 QPair<QPointF, QPointF>(culebrilla.getPointAtFrame(index), culebrilla.getPointAtFrame(lastFrame))
00303 );
00304 previousH[lastFrame] = ComputeProjectiveHomography(matchings)*previousH[index];
00305 setPropertyValue<QVMatrix>("Homography", pseudoInverse(previousH[lastFrame]));
00306 break;
00307 }
00308 else
00309 if (index == lastFrame)
00310 {
00311 std::cout << "ERROR, lost track of PLANE" << std::endl;
00312 const int rectificationFrame = getPropertyValue< int >("Frame for planar rectification homography");
00313 const QVMatrix rectificationHomography = getPropertyValue< QVMatrix >("Planar rectification homography");
00314 previousH[rectificationFrame] = rectificationHomography;
00315 break;
00316 }
00317 }
00318 };
00319
00320 int main(int argc, char *argv[])
00321 {
00322 QVApplication app(argc, argv,
00323 "Example program for QVision library. Finds a template pattern on the image."
00324 );
00325
00326 QVMPlayerCamera camera("Video");
00327
00328
00329 HessianCornersExtractor cornersExtractor("Hessian corners extractor");
00330 camera.link(&cornersExtractor,"Input image");
00331
00332
00333 FeatureTracker featureTracker("Feature tracker worker");
00334 cornersExtractor.linkProperty("Corner responses", &featureTracker, "Feature responses", QVWorker::SynchronousLink);
00335 cornersExtractor.linkProperty("Corner locations", &featureTracker, "Feature locations", QVWorker::SynchronousLink);
00336
00337
00338 PlanarDetector planarDetector("Planar detector");
00339 featureTracker.linkProperty("Culebrillas container", &planarDetector, "Culebrillas container", QVWorker::AsynchronousLink);
00340
00341 PlanarCharacterizator planarCharacterizator("Planar characterizator");
00342 planarDetector.linkProperty(&planarCharacterizator, QVWorker::SynchronousLink);
00343
00344
00345 PlanarTracker planarTracker("Planar Characterizer");
00346 featureTracker.linkProperty("Culebrillas container", &planarTracker, "Culebrillas container", QVWorker::SynchronousLink);
00347 planarCharacterizator.linkProperty(&planarTracker, QVWorker::AsynchronousLink);
00348
00349
00350 QVPlanarRectifierWorker warper("Image warper");
00351 cornersExtractor.linkProperty("Input image", &warper, "Input image", QVWorker::SynchronousLink);
00352 planarTracker.linkProperty("Homography", &warper, "Homography", QVWorker::SynchronousLink);
00353
00354
00355 QVGUI interface;
00356
00357
00358 QVImageCanvas imageCanvas2("Original");
00359 imageCanvas2.linkProperty(warper, "Input image");
00360
00361
00362 QVImageCanvas imageCanvas3("Warped");
00363 imageCanvas3.linkProperty(warper, "Warped image");
00364
00365 return app.exec();
00366 }
00367
00368 #endif
00369