examples/realtimePlanarRectificator/realtimePlanarRectificator.cpp

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 
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                                 // probar eficiencia de una y otra forma
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                         // RotZ[delta_] := {{Cos[delta], Sin[delta], 0}, {-Sin[delta], Cos[delta], 0}, {0, 0, 1}};
00117                         // RotX[delta_] := {{1, 0, 0}, {0, Cos[delta], Sin[delta]}, {0, -Sin[delta], Cos[delta]}};
00118                         // RotY[delta_] := {{Cos[delta], 0, -Sin[delta]}, {0, 1, 0}, {Sin[delta], 0, Cos[delta]}};
00119                         // RotZ[-rolll].RotX[tilt]
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                         // Propiedades compartidas
00142                         addProperty< int >("Frame for planar rectification homography", inputFlag|outputFlag);
00143                         addProperty< QSet<Culebrilla> >("Planar culebrillas", outputFlag);
00144                         }
00145 
00146                 void iterate()
00147                         {
00148                         // 0. Actualize iterate execution delay.
00149                         setMinimumDelay(getPropertyValue<int>("Minimum delay"));
00150 
00151                         // 1. Get culebrillas.
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                         // 2. Get culebrillas list with a minimum size of (actualFrame - minimumCulebrillaSize)
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                         //addProperty< int >("Actual frame number", inputFlag);
00181 
00182                         addProperty< QSet<Culebrilla> >("Planar culebrillas", inputFlag|outputFlag);
00183 
00184                         // Propiedades de salida del characterizator
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                         // Check that actual rectification homography is valid. Else recalculate
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                                 // 3.1. Get list of homographies, from actual frame to the oldest frame.
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                                 // 3.2. Calculate rectifying homography.
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                         // 0. Get input data.
00259                         const CulebrillaContainer culebrillaContainer = getPropertyValue< CulebrillaContainer >("Culebrillas container");
00260                         const QSet<Culebrilla> planarCulebrillas = getPropertyValue< QSet<Culebrilla> >("Planar culebrillas");
00261 
00262                         // 1. Get fresh planar Culebrillas.
00263                         QSet<Culebrilla> culebrillas;
00264                         foreach(Culebrilla culebrilla, culebrillaContainer.getCulebrillas())
00265                                 if (planarCulebrillas.contains(culebrilla))
00266                                         culebrillas.insert(culebrilla);
00267 
00268                         // 2. If not enough culebrillas, break;
00269                         if (culebrillas.size() < 8)
00270                                 {
00271                                 previousH.clear();
00272                                 setPropertyValue<QVMatrix>("Homography", QVMatrix::identity(3));
00273                                 return;
00274                                 }
00275 
00276                         // 2.2. Get first and last common frame for all test culebrillas.
00277                         int firstFrame = 0, lastFrame = culebrillaContainer.getActualFrameNumber();
00278                         foreach(Culebrilla culebrilla, culebrillas)
00279                                 firstFrame = MAX(firstFrame, culebrilla.getFirstFrameNumber());
00280 
00281                         // 3. If we can't get a match from the actual frame to the last rectified one,
00282                         //    recalculate rectifying matrix.
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                         // 4. Compute new rectifying homography
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         // Corner extraction
00329         HessianCornersExtractor cornersExtractor("Hessian corners extractor");
00330         camera.link(&cornersExtractor,"Input image");
00331 
00332         // Feature tracker
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         // Planar finder
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         // Homography extractor 
00345         PlanarTracker planarTracker("Planar Characterizer");
00346         featureTracker.linkProperty("Culebrillas container", &planarTracker, "Culebrillas container", QVWorker::SynchronousLink);
00347         planarCharacterizator.linkProperty(&planarTracker, QVWorker::AsynchronousLink);
00348 
00349         // Image warper
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         // Data output
00355         QVGUI interface;
00356 
00357         // Original image
00358         QVImageCanvas imageCanvas2("Original");
00359         imageCanvas2.linkProperty(warper, "Input image");
00360 
00361         // Warped image
00362         QVImageCanvas imageCanvas3("Warped");
00363         imageCanvas3.linkProperty(warper, "Warped image");
00364 
00365         return app.exec();
00366         }
00367 
00368 #endif
00369 

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