examples/homography/homography.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 
00042 #include <stdio.h>
00043 #include <stdlib.h>
00044 #include <iostream>
00045 #include <math.h>
00046 
00047 #include <QDebug>
00048 #include <QVector>
00049 
00050 #include <qvcore/qvapplication.h>
00051 #include <qvcameras/qvmplayercamera.h>
00052 #include <qvgui/qvgui.h>
00053 
00054 #include <qvdta/qvpolyline.h>
00055 #include <qvdta/qvdta.h>
00056 
00057 #include <TooN/TooN.h>
00058 #include <TooN/numerics.h>      // Basic TooN library
00059 #include <TooN/numhelpers.h>    // If you want a few extras
00060 #include <TooN/SVD.h>           // If you want singular value decompositions
00061 #include <TooN/LU.h>            // If you want lu triangular decompositions
00062 #include <TooN/SymEigen.h>      // If you want Eigen decomposition of symmetric matrices
00063 
00064 #define PI      3.14159
00065 
00066 Q_DECLARE_METATYPE(Matrix<>);
00067 
00069 void GetHotPoints(const QVImage<sFloat> cornerResponseImage, QList<QPoint> &hotPoints, uInt maxWindow)
00070         {
00071         const uInt rows = cornerResponseImage.getRows(), cols = cornerResponseImage.getCols();
00072 
00073         QVImage<uChar> binaryCornerImage(cols, rows);
00074         FilterLocalMax(cornerResponseImage, binaryCornerImage, maxWindow, maxWindow);
00075 
00076         QVIMAGE_INIT_READ(uChar,binaryCornerImage);
00077         for (uInt row = 0; row < binaryCornerImage.getRows(); row++)
00078                 for (uInt col = 0; col < binaryCornerImage.getCols(); col++)
00079                         if (QVIMAGE_PIXEL(binaryCornerImage, col, row,0))
00080                                 hotPoints.append(QPoint(col, row));
00081         }
00082 
00083 void GetMaximalPoints(const QVImage<sFloat> cornerResponseImage, QList<QPoint> &hotPoints, QList<QPoint> &maximalPoints, uInt maxPoints)
00084         {
00085         while( hotPoints.size() > 0 && maximalPoints.size() < maxPoints )
00086                 {
00087                 uInt maxIndex = 0;
00088                 for (int n=0; n < hotPoints.size(); n++)
00089                         if ( cornerResponseImage(hotPoints.at(n)) >  cornerResponseImage(hotPoints.at(maxIndex)) )
00090                                 maxIndex = n;
00091 
00092                 maximalPoints.append(hotPoints.at(maxIndex));
00093                 hotPoints.removeAt(maxIndex);
00094                 }
00095         }
00096 
00097 uInt    getClosestPointIndex(const QPoint point, const QList<QPoint> &pointList)
00098         {
00099         uInt index = 0;
00100         for (uInt n = 1; n < pointList.size(); n++)
00101                 if ((point - pointList.at(n)).manhattanLength() < (point - pointList.at(index)).manhattanLength())
00102                         index = n;
00103 
00104         return index;
00105         }
00106 
00107 QPoint  getMeanPoint(const QList<QPoint> &pointList)
00108         {
00109         QPoint center(0,0);
00110 
00111         for (uInt n = 0; n < pointList.size(); n++)
00112                 {
00113                 center.rx() += pointList.at(n).x();
00114                 center.ry() += pointList.at(n).y();
00115                 }
00116 
00117         center.rx() = center.rx() / pointList.size();
00118         center.ry() = center.ry() / pointList.size();
00119 
00120         return center;
00121         }
00122 
00123 double  angle(const QPoint &p)
00124         {
00125         double x = p.x(), y = p.y();
00126         if (x>0)
00127                 if (y>=0)
00128                         return atan(y/x);
00129                 else
00130                         return atan(y/x) + 2*PI;
00131         else if (x == 0)
00132                 if (y>0)
00133                         return PI/2;
00134                 else
00135                         return 3*PI/2;
00136         else // x < 0
00137                 return atan(y/x)+PI;
00138         }
00139 
00140 double  clockWiseAngle(const QPoint &p1, const QPoint &p2)
00141         {
00142         double clockAngle = angle(p2) - angle(p1);
00143         return (clockAngle < 0)? clockAngle + 2*PI:clockAngle;
00144         }
00145 
00146 // This function creates a list of the template points, first with the center point,
00147 // then the rest of the points following a clockwise order around the center point.
00148 QList<QPoint> GetTemplatePoints(uInt zoom, QPoint center)
00149         {
00150         QPoint rPoints[5] =
00151                 //      Center          Norwest         Noreast         Southeast       Southwest
00152                 {       QPoint(0,0),    QPoint(-1,+1),  QPoint(+1,+1),  QPoint(+1,-1),  QPoint(-1,-1)   };
00153 
00154         QList<QPoint> templatePoints;
00155 
00156         for (uInt i= 0; i < 5; i++)
00157                 templatePoints.append(zoom*rPoints[i]+center);
00158 
00159         return templatePoints;
00160         }
00161 
00162 // This function creates a list of the template points, first with the center point,
00163 // then the rest of the points following a clockwise order around the center point.
00164 Matrix<> GetTemplateMatrixPoints()
00165         {
00166         //                                      Center          Norwest         Noreast         Southeast       Southwest
00167         return Matrix<5,3> ((double[5][3]){     0,0,1,          -1,+1,+1,       +1,+1,+1,       +1,-1,+1,       -1,-1,+1});
00168         }
00169 
00170 // This is suposed to sort the points in a list, by... who knows?
00171 // Anyway, it doesn't works
00172 bool SortTemplatePoints(QList<QPoint> &points)
00173         {
00174         if (points.size() != 5)
00175                 return false;
00176 
00177         QList<QPoint> result;
00178 
00179         // This array will keep the index of the points.
00180         uInt index[5];
00181 
00182         // Calculate the index of the central point
00183         uInt indexp = getClosestPointIndex(getMeanPoint(points), points);
00184 
00185         result.append(points.at(indexp));
00186         points.removeAt(indexp);
00187 
00188         // Calculate the index of the top left point
00189         double minDistance = 1000000;
00190         for (uInt n = 0; n < points.size(); n++)
00191                 if ( points.at(n).manhattanLength() < minDistance )
00192                         {
00193                         minDistance = points.at(n).manhattanLength();
00194                         indexp = n;
00195                         }
00196 
00197         result.append(points.at(indexp));
00198         points.removeAt(indexp);
00199 
00200         // Calculate the other points
00201         while(points.size() > 0)
00202                 {
00203                 indexp = 0;
00204                 double minAngle = clockWiseAngle( result.back() - result.front(), points.at(indexp) - result.front());
00205         
00206                 for (uInt n = 1; n < points.size(); n++)
00207                         {
00208                         double actualAngle = clockWiseAngle( result.back() - result.front(), points.at(n) - result.front());
00209                         if ( actualAngle < minAngle )
00210                                 {
00211                                 minAngle = actualAngle;
00212                                 indexp = n;
00213                                 }
00214                         }
00215 
00216                 result.append(points.at(indexp));
00217                 points.removeAt(indexp);
00218                 }
00219 
00220         points = result;
00221 
00222         return true;
00223         }
00224 
00225 
00226 // This function returns the homography that maps the points from a source position to a
00227 // destination position
00228 Matrix<> CalibrateHomography(const Matrix<> &sourcePointsMatrix, const Matrix<> &destinationPoints)
00229         {
00230         Q_ASSERT(sourcePointsMatrix.num_cols() == 2);
00231         Q_ASSERT(sourcePointsMatrix.num_cols() == destinationPoints.num_cols());
00232         Q_ASSERT(sourcePointsMatrix.num_rows() == destinationPoints.num_rows());
00233 
00234         const uInt rows = sourcePointsMatrix.num_rows();
00235 
00236         // First, state the coefs matrix for the homogeneous linear system of equations, of the form:
00237         //      Ax = 0
00238         Matrix<> coefsMatrix(3*rows,9);
00239 
00240         //std::cout << "Points" << std::endl;
00241         for (uInt n = 0; n < rows; n++)
00242                 {
00243                 double  x = sourcePointsMatrix[n][0], y = sourcePointsMatrix[n][1],
00244                         p = destinationPoints[n][0], q = destinationPoints[n][1];
00245 
00246                 double  equation1[9] = {        0,      0,      0,      -x,     -y,     -1,     q*x,    q*y,    q},
00247                         equation2[9] = {        x,      y,      1,      0,      0,      0,      -p*x,   -p*y,   -p},
00248                         equation3[9] = {        -q*x,   -q*y,   -q,     p*x,    p*y,    p,      0,      0,      0};
00249 
00250                 coefsMatrix[3*n] = Vector<9>(equation1);
00251                 coefsMatrix[3*n+1] = Vector<9>(equation2);
00252                 coefsMatrix[3*n+2] = Vector<9>(equation3);
00253                 }
00254 
00255         // create the SVD of M
00256         SVD<> svdCoefsMatrix(coefsMatrix);
00257 
00258         Vector<9> x =  svdCoefsMatrix.get_VT()[8];
00259         Matrix<> homography(3,3);
00260 
00261         homography[0][0] = x[0];        homography[0][1] = x[1];        homography[0][2] = x[2];
00262         homography[1][0] = x[3];        homography[1][1] = x[4];        homography[1][2] = x[5];
00263         homography[2][0] = x[6];        homography[2][1] = x[7];        homography[2][2] = x[8];
00264 
00265         return homography;
00266         }
00267 
00268 // This function maps a point given a planar homography
00269 QPoint  applyHomography(const QPoint p, const Matrix<> homography)
00270         {
00271         const double values[3] = { p.x(), p.y(), 1 };
00272         Vector<3> p1 = Vector<3>(values), p2 = homography * p1;
00273         return QPoint(round(p2[0]/p2[2]), round(p2[1]/p2[2]));
00274         }
00275 
00276 void normalizeHomogeneousCoordinates(Matrix<> &points)
00277         {
00278         const uInt cols = points.num_cols(), rows = points.num_rows();
00279 
00280         for (uInt i = 0; i < rows; i++)
00281                 for (uInt j = 0; j < cols; j++)
00282                         points[i][j] /= points[i][cols-1];
00283         }
00284 
00285 // This function retunrs an integer value that indicates how well the homography maps the source points
00286 // in a list, to the points in the destination list.
00287 double  testErrorHomography(const Matrix<> &sourcePointsMatrix, const Matrix<> &destinationPoints, const Matrix<> homography)
00288         {
00289         const uInt cols = sourcePointsMatrix.num_cols(), rows = sourcePointsMatrix.num_rows();
00290 
00291         Matrix <> projectedPoints(sourcePointsMatrix.num_rows(),3);
00292         Matrix <> residuals (sourcePointsMatrix.num_rows(),3);
00293 
00294         projectedPoints = sourcePointsMatrix * homography.T();
00295         normalizeHomogeneousCoordinates(projectedPoints);
00296         residuals = projectedPoints - destinationPoints;
00297 
00298         double accum = 0;
00299         for (uInt i = 0; i < rows; i++)
00300                 {
00301                 double square = 0;
00302                 for (uInt j = 0; j < cols; j++)
00303                         square += residuals[i][j]*residuals[i][j];
00304                 accum += sqrt(square);
00305                 }
00306 
00307         return accum;
00308         }
00309 
00310 // This function sustitudes warpPerspective from the qvipp, and applies the planar transformation H
00311 // to a source image, storing the transformed image in the destination image
00312 void    myWarpPerspective(const QVImage<uChar> &src, QVImage<uChar> &dest, const Matrix <> H, int zoom)
00313         {
00314         const uInt cols = src.getCols(), rows = src.getRows();
00315         const QPoint center = QPoint(cols/2, rows/2);
00316         const Matrix<> Hinv = SVD<>(H).get_pinv();
00317         const double meanColsRows = ((double)(cols + rows)) / 2;
00318 
00319         for (uInt col = 0; col < cols; col++)
00320                 for (uInt row = 0; row < rows; row++)
00321                         {
00322                         Vector<3> v;
00323                         v[0] = col - cols/2;
00324                         v[1] = row - rows/2;
00325                         v[2] = meanColsRows;
00326                         Vector<3>vP;
00327                         vP = H*v;
00328                         QPoint p2 =  QPoint(round(zoom*vP[0]/vP[2]),round(zoom*vP[1]/vP[2]))+center, p1 = QPoint(col, row);
00329                         if (dest.getROI().contains(p2))
00330                                 dest(p2) = src(p1);
00331                         }
00332         }
00333 
00334 // Given the four corners of a square, transformed by an homography, this function obtains two points
00335 // of the line of the horizont.
00336 bool getHorizont(const double x1, const double y1, const double x2, const double y2, const double x3, const double y3, const double x4, const double y4,
00337                 double &x5, double &y5, double &x6, double &y6)
00338         {
00339         const double    x5num =         x1 *(x4 *(-y2 + y3) + x3 *(y2 - y4)) + x2 *(x4 *(y1 - y3) + x3 *(-y1 + y4)),
00340                         x5denom =       x4 *(y1 - y2) + x3 *(-y1 + y2) + (x1 - x2)* (y3 - y4),
00341                         y5num =         x4 *(y1 - y2)* y3 + x1* y2* y3 - x3* y1* y4 - x1* y2* y4 + x3 *y2* y4 + x2 *y1 *(-y3 + y4),
00342                         y5denom =       x4 *(y1 - y2) + x3 *(-y1 + y2) + (x1 - x2)* (y3 - y4),
00343 
00344                         x6num =         x3 *(x4 *(y1 - y2) + x1 *(y2 - y4)) + x2* (x4 *(-y1 + y3) + x1 *(-y3 + y4)),
00345                         x6denom =       (x1 - x4) *(y2 - y3) + x3 *(y1 - y4) + x2 *(-y1 + y4),
00346                         y6num =         -x2* y1* y3 + x4* y1* (-y2 + y3) + x3* y2* (y1 - y4) + x1* y2* y4 - x1* y3* y4 + x2* y3* y4,
00347                         y6denom =       (x1 - x4)* (y2 - y3) + x3* (y1 - y4) + x2* (-y1 + y4);
00348 
00349         if (x5denom == 0 || y5denom == 0 || x6denom == 0 || y6denom == 0)
00350                 return false;
00351 
00352         x5 = x5num / x5denom;
00353         y5 = y5num / y5denom;
00354         x6 = x6num / x6denom;
00355         y6 = y6num / y6denom;
00356 
00357         return true;
00358         }
00359 
00360 // This function gets the closest point in a line, to a given point, not contained in it
00361 bool getPerpendicularPoint(const double xc, const double yc, const double x5, const double y5, const double x6, const double y6, double &x7, double &y7)
00362         {
00363         const double    x7num =         xc*x5*x5 + x6*(xc*x6-(yc-y5)*(y5-y6)) + x5*(-2*xc*x6+(yc-y6)*(y5-y6)),
00364                         x7denom =       x5*x5-2*x5*x6+x6*x6+(y5-y6)*(y5-y6),
00365                         y7num =         x6*x6*y5+yc*(y5-y6)*(y5-y6)+x5*x5*y6+xc*x6*(-y5+y6)+x5*((xc-x6)*y5-(xc+x6)*y6),
00366                         y7denom =       x5*x5 - 2*x5*x6 + x6*x6 + (y5-y6)*(y5-y6);
00367 
00368 
00369         if (x7denom == 0 || y7denom == 0)
00370                 return false;
00371 
00372         x7 = x7num / x7denom;
00373         y7 = y7num / y7denom;
00374 
00375         return true;
00376 
00377         }
00378 
00379 // This function draws a line in an image, given two points of it.
00380 void    myDrawLine(QVImage<uChar,3> &image, const int x1, const int y1, const int x2, const int y2)
00381         {
00382         const uInt rows = image.getRows(), cols = image.getCols();
00383         const double m = (double)(y2-y1)/(double)(x2-x1);
00384 
00385         for(int x = 0; x< cols; x++)
00386                 {
00387                 int y = m*(x - x1) + y1;
00388                 if ( y >= 0 && y < rows)
00389                         {
00390                         image(x,y,0) = 255;
00391                         image(x,y,1) = 0;
00392                         image(x,y,2) = 0;
00393                         }
00394                 }
00395         }
00396 
00397 // This function transforms a list of n points, to a nx3 matrix, containing the rows the
00398 // homogeneous coordinates for each point in the list.
00399 void pointListToMatrix(const QList<QPoint> &points, Matrix<> &matrix)
00400         {
00401         Q_ASSERT(points.size() == matrix.get_rows());
00402         Q_ASSERT(3 == matrix.get_cols());
00403 
00404         for (uInt n = 0; n < points.size(); n++)
00405                 {
00406                 double  v[3] = { points.at(n).x(), points.at(n).y(), 1 };
00407                 matrix[n] = Vector<3>(v);
00408                 }
00409         }
00410 
00411 #define NORM(x,y)       (sqrt((x)*(x) + (y)*(y)))
00412 
00413 void getSubpixelPrecissionPoints(const QVImage<sFloat> &weightImage, QList<QPoint> &pointList, Matrix<> &pointsMatrix, uInt margin)
00414         {
00415         const uInt      rows = weightImage.getRows(), cols = weightImage.getCols();
00416 
00417         QVIMAGE_INIT_READ(sFloat, weightImage);
00418         for (uInt n = 0; n < pointList.size(); n++)
00419                 {
00420                 const QPoint p = pointList.at(n);
00421                 const int x = p.x(), y = p.y();
00422                 if (x > margin && x < cols-margin && y > margin && y < rows-margin)
00423                         {
00424                         double v[3] = {0, 0, 1}, sum = 0;
00425                         for (uint i = x-margin; i<=x+margin; i++)
00426                                 for (uint j = y-margin; j<=y+margin; j++)
00427                                         {
00428                                         const double factor = QVIMAGE_PIXEL(weightImage, i, j, 0);
00429                                         v[0] += i*factor;
00430                                         v[1] += j*factor;
00431                                         sum += factor;
00432                                         }
00433 
00434                         v[0] = v[0] / sum;
00435                         v[1] = v[1] / sum;
00436 
00437                         pointsMatrix[n] = Vector<3>(v);
00438                         }
00439                 }
00440         }
00441 
00442 // Gets the minimum point of the parabol that contains the three points (x1,y1), (x2,y2), (x3,y3). In Mathematica:
00443 // Solve[(2a x + b /. Solve[{   a x1^2 + b  x1 + c == y1,
00444 //                              a x2^2 + b  x2 + c == y2,
00445 //                              a x3^2 + b  x3 + c == y3        },
00446 //      {a, b, c}]) == 0, x]
00447 double minimumAtCuadraticInterpolation(const double x1, const double y1, const double x2, const double y2, const double x3, const double y3)
00448         {
00449         const double    numerator = x2*x2*y1 - x3*x3*y1 - x1*x1*y2 + x3*x3*y2 + x1*x1*y3 - x2*x2*y3,
00450                         denominator =  x2*y1 - x3*y1 - x1*y2 + x3*y2 + x1*y3 - x2*y3;
00451 
00452         return numerator/(2*denominator);
00453         }
00454 
00455 #define POW2(X) ((X)*(X))
00456 #define ABS(X)  ((X>0)?(X):(-X))
00457 
00458 // Error function, given an 'f', and a 'H' matrix. In Mathematica:
00459 //      K = {{f, 0, 0}, {0, f, 0}, {0, 0, 1}};
00460 //      Hvar = {{h1, h2, h3}, {h4, h5, h6}, {h7, h8, h9}};
00461 //      ErrorMat[H_, K_] := Transpose[H].Inverse[K].Inverse[K].H;
00462 //      Error [ErrorM_] := ((ErrorM[[1, 1]] - ErrorM[[2, 2]])^2 + ErrorM[[1, 2]]^ 2) / ErrorM[[1, 1]]^ 2;
00463 //      ErrorFunction[H_, K_] := Error[ErrorMat[H, K]];
00464 //      ErrorFunction[Hvar, K] // Simplify
00465 #define ERROR(f)                                                                \
00466         (       (                                                               \
00467                 POW2( h1*h2 + h4*h5 + (f)*(f)*h7*h8 ) +                         \
00468                 POW2( h1*h1 - h2*h2 + h4*h4 - h5*h5 + (f)*(f)*(h7*h7-h8*h8) )   \
00469                 ) / POW2(h1*h1 + h4*h4 + (f)*(f)*h7*h7)                         \
00470         )
00471 double errorHomographyFocalDistance(const Matrix <3,3> &H, const double f)
00472         {
00473         const double    h1 = H[0][0],   h2 = H[0][1],   h3 = H[0][2],
00474                         h4 = H[1][0],   h5 = H[1][1],   h6 = H[1][2],
00475                         h7 = H[2][0],   h8 = H[2][1],   h9 = H[2][2];
00476         return ERROR(f);
00477         }
00478 
00479 double errorHomographiesFocalDistance(const QVector< Matrix <3,3> > &Hlist, const double f)
00480         {
00481         double error = 0;
00482         for (uInt n = 0; n< Hlist.size(); n++)
00483                 {
00484                 Matrix<3,3> H = Hlist[n];
00485                 const double    h1 = H[0][0],   h2 = H[0][1],   h3 = H[0][2],
00486                                 h4 = H[1][0],   h5 = H[1][1],   h6 = H[1][2],
00487                                 h7 = H[2][0],   h8 = H[2][1],   h9 = H[2][2];
00488                 error +=        ( ( POW2( h1*h2 + h4*h5 + (f)*(f)*h7*h8 ) + POW2( h1*h1 - h2*h2 + h4*h4 - h5*h5 + (f)*(f)*(h7*h7-h8*h8) ) ) / POW2(h1*h1 + h4*h4 + (f)*(f)*h7*h7) );
00489                 }
00490 
00491         return error;
00492         }
00493 
00494 // This function finds the fov that minimices errorHomographyFocalDistance for a given homography H,
00495 // in a neighbourhood of a fov value passed in the variable 'startingFov'.
00496 double calibrateFovDistance(const Matrix<> &Horig, const double startingFov = 3)
00497         {
00498         Matrix <3,3> H;
00499 
00500         // We will work with the inverse of Horig
00501         H = SVD<>(Horig).get_pinv();
00502         double f = startingFov;
00503         
00504         for (uInt i = 0; i < 10; i++)
00505                 {
00506                 f = minimumAtCuadraticInterpolation(
00507                                         f, errorHomographyFocalDistance(H,f),
00508                                         f-0.1, errorHomographyFocalDistance(H,f-0.1),
00509                                         f+0.1, errorHomographyFocalDistance(H,f+0.1)
00510                                         );
00511                 }
00512         //std::cout << i << " f = " << f << ", ERROR = " << errorHomographyFocalDistance(H,f) << std::endl;
00513         return f;
00514         }
00515 
00516 bool minimizeFovErrorFunction(const QVector< Matrix<3,3> > &H, double &f, const uInt maxSteps = 100)
00517         {
00518         double startError = errorHomographiesFocalDistance(H,f);
00519         double lastF = f, lastError = startError;
00520 
00521         for (uInt i = 0; i < maxSteps; i++)
00522                 {
00523                 f = minimumAtCuadraticInterpolation(
00524                                         f, errorHomographiesFocalDistance(H,f),
00525                                         f-0.1, errorHomographiesFocalDistance(H,f - 0.1),
00526                                         f+0.1, errorHomographiesFocalDistance(H,f + 0.1)
00527                                         );
00528 
00529                 const double error = errorHomographiesFocalDistance(H,f);
00530 
00531                 std::cout << i << " f = " << f << ", ERROR = " << error << std::endl;
00532 
00533                 if (error > startError)
00534                         return false;
00535 
00536                 if (error > lastError)
00537                         {
00538                         f = lastF;
00539                         return true;
00540                         }
00541 
00542                 lastF = f;
00543                 lastError = error;
00544                 }
00545 
00546         return false;
00547         }
00548 
00549 double calibrateFovDistance(const QVector< Matrix<3,3> > &Horig, const uInt maxSteps = 100)
00550         {
00551         QVector< Matrix<3,3> > H;
00552         H.resize(Horig.size());
00553 
00554         
00555         // We will work with the inverse of Horig's
00556         for(uInt n=0; n< Horig.size(); n++)
00557                 H[n] = SVD<>(Horig[n]).get_pinv();
00558 
00559         for (double f0 = 0.5; f0 < 10; f0 += 0.5)
00560                 {
00561                 double f = f0;
00562                 if (minimizeFovErrorFunction(H, f, maxSteps))
00563                         return f;
00564                 }
00565 
00566         return 0.0;
00567         }
00568 
00569 #define MAX_BEST_MATRICES       12
00570 
00571 class MyWorker: public QVWorker
00572         {
00573         //Matrix<3,3> matrixList[MAX_BEST_MATRICES];
00574         double  matrixError[MAX_BEST_MATRICES];
00575         QVector< Matrix<3,3> > matrixList;
00576 
00577         public:
00578                 MyWorker(QString name): QVWorker(name)
00579                         {
00580                         matrixList.resize(MAX_BEST_MATRICES);
00581                         for(uInt n=0; n<MAX_BEST_MATRICES; n++)
00582                                 matrixError[n] = 100;
00583 
00584                         addProperty<double>("Max error", inputFlag, 0.02, "window size", 0, 0.1);
00585                         addProperty<double>("Search starting FOV", inputFlag, 3.5, "window size", 0, 16);
00586 
00587                         addProperty<int>("Zoom", inputFlag, 50, "window size", 1, 256);
00588                         addProperty<int>("Window size", inputFlag, 10, "window size", 1, 100);
00589                         addProperty<int>("Margin", inputFlag, 1, "window size", 1, 64);
00590                         addProperty< QVImage<uChar,1> >("Input image", inputFlag|outputFlag);
00591                         addProperty< QVImage<uChar,3> >("Corners", outputFlag);
00592                         addProperty< QVImage<uChar,1> >("Wrapped", outputFlag);
00593                         addProperty< QVImage<uChar,1> >("Wrapped2", outputFlag);
00594                         addProperty< QVImage<sFloat,1> >("Response image", outputFlag);
00595 
00596                         addTrigger("Grab homography");
00597                         addProperty< Matrix<> >("Actual homography", outputFlag);
00598                         }
00599 
00600                 void processTrigger(QString triggerName)
00601                         {
00602                         /*std::cout <<  "H = {"
00603                                         << "{" << H[0][0] << ", " << H[0][1] << ", " << H[0][2] << "},"
00604                                         << "{" << H[1][0] << ", " << H[1][1] << ", " << H[1][2] << "},"
00605                                         << "{" << H[2][0] << ", " << H[2][1] << ", " << H[2][2] << "}};" << std::endl;
00606 
00607                         std::cout << "--------- Best homographies ----------" << std::endl;
00608 
00609                         for (uInt n=0; n<MAX_BEST_MATRICES; n++)
00610                                 {
00611                                 Matrix<3,3> H;
00612                                 H = matrixList[n];
00613                                 std::cout <<    "H" << n <<" = {"
00614                                         << "{" << H[0][0] << ", " << H[0][1] << ", " << H[0][2] << "},"
00615                                         << "{" << H[1][0] << ", " << H[1][1] << ", " << H[1][2] << "},"
00616                                         << "{" << H[2][0] << ", " << H[2][1] << ", " << H[2][2] << "}}; (* " << matrixError[n] << "*)" << std::endl;
00617                                 }*/
00618 
00619                         //std::cout << "startingFov = " << f << std::endl;
00620                         double fov = calibrateFovDistance(matrixList);
00621                         std::cout << "FOV[" << MAX_BEST_MATRICES << "] = " << fov << std::endl;
00622                         std::cout << "FOV = " << calibrateFovDistance(getPropertyValue< Matrix<> >("Actual homography"),fov+1) << std::endl;
00623                         };
00624 
00625                 void iterate()
00626                         {
00627                         const QVImage<uChar> image = getPropertyValue< QVImage<uChar,1> >("Input image");
00628                         const uInt      rows = image.getRows(), cols = image.getCols(),
00629                                         sizeMax = getPropertyValue<int>("Window size"),
00630                                         zoom = getPropertyValue<int>("Zoom"),
00631                                         margin = getPropertyValue<int>("Margin");
00632                         const double    maxError = getPropertyValue<double>("Max error");
00633 
00634                         QVImage<uChar,3> destino = image;
00635 
00636                         timeFlag("grab Frame");
00637 
00639                         // Harris corner response image
00640                         QVImage<sFloat> temp(cols, rows), cornerResponseImage(cols, rows);
00641                         //HarrisCornerResponseImage(image, cornerResponseImage);
00642                         SobelCornerResponseImage(image, cornerResponseImage);
00643 
00644                         timeFlag("Corner response image");
00645 
00646                         //setPropertyValue< QVImage<sFloat> >("Response image", cornerResponseImage);
00647 
00649                         // Hot points
00650                         QList<QPoint> hotPoints;
00651                         GetHotPoints(cornerResponseImage, hotPoints, sizeMax);
00652 
00653                         timeFlag("Get hotpoints");
00654 
00656                         // Calibración
00657                         QList<QPoint> maximalPoints, templatePoints = GetTemplatePoints(1, QPoint(0, 0));
00658                         GetMaximalPoints(cornerResponseImage, hotPoints, maximalPoints, 5);
00659                         SortTemplatePoints(maximalPoints);
00660 
00661                         timeFlag("Get max hotpoints");
00662 
00663                         if (maximalPoints.size() == 5)
00664                                 {
00665                                 Matrix <> sourcePointsMatrix(5,3), sourcePointsMatrix2(5,3), destinationPointsMatrix(5,3);
00666                                 destinationPointsMatrix =  GetTemplateMatrixPoints();
00667                                 getSubpixelPrecissionPoints(cornerResponseImage, maximalPoints, sourcePointsMatrix,margin);
00668                                 pointListToMatrix(maximalPoints, sourcePointsMatrix2);
00669 
00670                                 //pointListToMatrix(templatePoints, destinationPointsMatrix);
00671 
00672                                 const double meanColsRows = cols/2;
00673                                 for (uInt i = 0; i<5; i++)
00674                                         {
00675                                         sourcePointsMatrix[i][0] = (sourcePointsMatrix[i][0] - cols/2)/ meanColsRows;
00676                                         sourcePointsMatrix[i][1] = -(sourcePointsMatrix[i][1] - rows/2)/ meanColsRows;
00677                                         sourcePointsMatrix2[i][0] = (sourcePointsMatrix2[i][0] - cols/2)/ meanColsRows;
00678                                         sourcePointsMatrix2[i][1] = -(sourcePointsMatrix2[i][1] - rows/2)/ meanColsRows;
00679                                         }
00680 
00682 
00683                                 Matrix<> H = CalibrateHomography(sourcePointsMatrix, destinationPointsMatrix);
00684                                 Matrix<> H2 = CalibrateHomography(sourcePointsMatrix2, destinationPointsMatrix);
00685 
00686                                 /*std::cout << "--------------------------" << std::endl;
00687                                 std::cout << "ERROR 1 " << testErrorHomography(sourcePointsMatrix, destinationPointsMatrix, H) << std::endl;
00688                                 std::cout << "ERROR 2 " << testErrorHomography(sourcePointsMatrix2, destinationPointsMatrix, H2) << std::endl;*/
00689 
00690                                 const double actualError = testErrorHomography(sourcePointsMatrix, destinationPointsMatrix, H);
00691                                 if (actualError < maxError)
00692                                         {
00693                                         setPropertyValue< Matrix<> >("Actual homography", H);
00694 
00695                                         uInt index = 0;
00696                                         for (uInt i=1; i<MAX_BEST_MATRICES; i++)
00697                                                 if (matrixError[i] > matrixError[index])
00698                                                         index = i;
00699 
00700                                         if (matrixError[index] > actualError)
00701                                                 {
00702                                                 matrixList[index] = H;
00703                                                 matrixError[index] = actualError;
00704                                                 }
00705 
00706 
00707                                         QVImage<uChar> wrapped(cols, rows);
00708                                         Set(wrapped,0);
00709         
00710                                         myWarpPerspective(image, wrapped, H, zoom);
00711 
00713                                         // Warp perspective using IPP 
00714                                         /*double coeffs[3][3]; 
00715                                         for (uInt i = 0; i<3; i++) 
00716                                                 for (uInt j=0; j<3; j++) 
00717                                                         coeffs[i][j] = H[i][j]; 
00718                                         WarpPerspective(image, wrapped, coeffs);*/
00720 
00721                                         QVImage<uChar> wrapped2(cols, rows);
00722                                         Set(wrapped2,0);
00723         
00724                                         myWarpPerspective(image, wrapped2, H2,zoom);
00725 
00726                                         QVImage<uChar> diff(cols, rows);
00727                                         AbsDiff(wrapped, wrapped2, diff);
00728                                         setPropertyValue< QVImage<uChar,1> >("Wrapped", wrapped);
00729                                         setPropertyValue< QVImage<uChar,1> >("Wrapped2", wrapped2);
00730 
00731                                         drawPoints(maximalPoints, destino);
00732                                         //std::cout << "FOV = " << calibrateFovDistance(H) << std::endl;
00733                                         }
00734                                 }
00735 
00736                         timeFlag("Calibrate");
00737 
00738                         // Fin rectificación ////////////////////////
00739 
00740                         setPropertyValue< QVImage<uChar,3> >("Corners", destino);
00741                         timeFlag("Draw corners");
00742                 
00743                         }
00744         };
00745 
00746 int main(int argc, char *argv[])
00747         {
00748         QVApplication app(argc, argv,
00749                 
00750                 "Example program for QVision library. Applies corner detection over an input video."
00751 
00752                 );
00753 
00754         QVMPlayerCamera camera("Video");
00755         MyWorker worker("Corners Worker");
00756         camera.link(&worker,"Input image");
00757 
00758         QVGUI interface;
00759 
00760         QVImageCanvas imageCanvas("Corners");
00761         imageCanvas.linkProperty(worker, "Corners");
00762 
00763         QVImageCanvas imageCanvas2("Wrapped");
00764         imageCanvas2.linkProperty(worker, "Wrapped");
00765 
00766         QVImageCanvas imageCanvas4("Wrapped 2");
00767         imageCanvas4.linkProperty(worker, "Wrapped2");
00768 
00769         return app.exec();
00770         }
00771 
00773 
00774 
00776 /*QVImage<uChar> buffer;
00777 QList<QPoint> pointList;
00778 FindPeaks3x3GetBufferSize(cornerResponseImage, buffer);         
00779 FindPeaks3x3(cornerResponseImage, buffer, pointList, 0.0, ippiNormInf, 1, hotPoints.size());
00780 timeFlag("Find Peaks");*/
00781 
00782 // Solucion en el Mathematica:
00783 //      f = a0 + a1 x + a2 y + a3 x y + a4 x^2 + a5 y^2;
00784 //      Solve[{D[f, x] == 0, D[f, y] == 0} // Simplify, {x, y}] // Simplify
00785 /*bool minimumAtCuadraticInterpolation(const Matrix <> &X, const Vector<> &y, Vector<> &xDest)
00786         {
00787         Matrix<> A(X.num_rows(),6);
00788 
00789         for (uInt n = 0; n < X.num_rows(); n++)
00790                 {
00791                 const double    x0 = X[n][0], x1 = X[n][1];
00792                 double          row[6] = {      1,      x0,     x1,     x0*x1,  x0*x0, x1*x1    };
00793                 A[n] = Vector<6>(row);
00794                 }
00795 
00796         Vector<> a = SVD<>(A).get_pinv()*y;
00797 
00798         const double denom = a[3]*a[3] - 4*a[4]*a[5];
00799         if (denom == 0)
00800                 return false;
00801 
00802         xDest[0] = (-a[2]*a[3]+2*a[1]*a[5]) / denom;
00803         xDest[1] = (-a[1]*a[3]+2*a[2]*a[4]) / denom;
00804 
00805         return true;
00806         }
00807 
00808 // Solucion en el Mathematica:
00809 //      f = a0 + a1 x + a2 y + a3 x^2 + a4 y^2;
00810 //      Solve[{D[f, x] == 0, D[f, y] == 0} // Simplify, {x, y}] // Simplify
00811 bool minimumAtCuadraticInterpolation2(const Matrix <> &X, const Vector<> &y, Vector<> &xDest)
00812         {
00813         Matrix<> A(X.num_rows(),5);
00814 
00815         for (uInt n = 0; n < X.num_rows(); n++)
00816                 {
00817                 const double    x0 = X[n][0], x1 = X[n][1];
00818                 double          row[5] = {      1,      x0,     x1,     x0*x0, x1*x1    };
00819                 A[n] = Vector<5>(row);
00820                 }
00821 
00822         Vector<> a = SVD<>(A).get_pinv()*y;
00823 
00824         xDest[0] = -a[1]/(2*a[3]);
00825         xDest[1] = -a[2]/(2*a[4]);
00826 
00827         return true;
00828         }*/
00829 
00830 // Given a pixel in an image, this function gets the coordinates of the points and the gray-level value
00831 // in the image of the points in a square neighbourhood of the pixel, of size 2*margin+1
00832 /*bool getMatrices(const QVImage<sFloat> &image, const QPoint p, Matrix<> &X, Vector<> &y, uInt margin)
00833         {
00834         const uInt      cols = image.getCols(), rows = image.getRows(),
00835                         col = p.x(), row = p.y();
00836 
00837         if (col < margin || col > cols -margin || row < margin || row > rows -margin)
00838                 return false;
00839 
00840         for (uInt i = col - 2, index = 0; i <= col +2; i++)
00841                 for (uInt j = row - 2; j <= row +2; j++, index++)
00842                         {
00843                         double row[2] = {i, j};
00844                         X[index] = Vector<2>(row);
00845                         y[index] = image(i, j);
00846                         }
00847 
00848         return true;
00849         }
00850 
00851 void getSubpixelPrecissionPoints2(const QVImage<sFloat> &weightImage, QList<QPoint> &pointList, Matrix<> &pointsMatrix)
00852         {
00853         for (uInt n = 0; n < maximalPoints.size(); n++)
00854                 {
00855                 Matrix<> X(25,2);
00856                 Vector <> y(25), x0(2);
00857                 getMatrices(cornerResponseImage, maximalPoints.at(n), X, y,0);
00858 
00859                 if (minimumAtCuadraticInterpolation2(X, y, x0))
00860                         sourcePointsMatrix[n] = x0;
00861                 else    {
00862                         sourcePointsMatrix[n][0] = -100;
00863                         sourcePointsMatrix[n][1] = -100;
00864                         }
00865                 //std::cout << "Matrix X" << X << std::endl;
00866                 //std::cout << "Matrix y" << y << std::endl;
00867                 
00868                 std::cout << "vector " << maximalPoints.at(n).x() << ", " << maximalPoints.at(n).y() << "\t" << sourcePointsMatrix[n] << std::endl;
00869                 }
00870 */
00871 
00872 /*// This function retunrs an integer value that indicates how well the homography maps the source points
00873 // in a list, to the points in the destination list.
00874 uInt    testErrorHomography(const QList<QPoint> &sourcePointsMatrix, const QList<QPoint> &destinationPoints, const Matrix<> homography)
00875         {
00876         uInt accum = 0;
00877         for (uInt i = 0; i < sourcePointsMatrix.size(); i++)
00878                 accum += (applyHomography(sourcePointsMatrix.at(i), homography) -destinationPoints.at(i)).manhattanLength();
00879 
00880         return accum;
00881         }*/

Generated on Fri Feb 22 18:26:55 2008 for QVision by  doxygen 1.5.3