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