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

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