examples/realtimePlanarRectificator/realtimePlanarRectificator.cpp

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