examples/calibrate3d/calibrate3d.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 #include <QThread>
00050 
00051 #include <qvcore/qvapplication.h>
00052 #include <qvcameras/qvmplayercamera.h>
00053 #include <qvgui/qvgui.h>
00054 
00055 #include <qvdta/qvpolyline.h>
00056 #include <qvdta/qvdta.h>
00057 
00058 #include <TooN/TooN.h>
00059 #include <TooN/numerics.h>      // Basic TooN library
00060 #include <TooN/numhelpers.h>    // If you want a few extras
00061 #include <TooN/SVD.h>           // If you want singular value decompositions
00062 #include <TooN/LU.h>            // If you want lu triangular decompositions
00063 #include <TooN/SymEigen.h>      // If you want Eigen decomposition of symmetric matrices
00064 
00065 #define PI      3.1415926535
00066 
00067 Q_DECLARE_METATYPE(Matrix<>);
00068 
00070 
00072 
00073 #include <GL/glut.h>
00074 #include <math.h>
00075 float phi, theta;
00076 //static float angle;
00077 int keyboard;
00078 int last_x, last_y;
00079 
00080 double x, y, z;
00081 GLfloat rot[16] = {1, 0, 0, 0,  0, 1, 0, 0,  0, 0, 1, 0,  0, 0, 0, 1};
00082 GLfloat tra[16] = {1, 0, 0, -2,  0, 1, 0, 0,  0, 0, 1, -6,  0, 0, 0, 1};
00083 GLfloat rot45[16] = {0.707, -0.707, 0, 0,  0.707, 0.707, 0, 0,  0, 0, 1, 0,  0, 0, 0, 1}; 
00084 
00085 void init(void) {
00086         // white ambient light
00087         GLfloat lightPosition[] = {30.0f, 0.0f, 3.0f, 0.0f};
00088         GLfloat shininess[] = {50};
00089         GLfloat whiteLight[] = {1.0f, 1.0f, 1.0f, 1.0f};
00090         GLfloat ambient[] = {0.1f, 0.1f, 0.1f, 0.1f};
00091         GLfloat specular[] = {1.0f, 1.0f, 1.0f, 1.0f};
00092         //glLightModelfv(GL_FRONT, ambientLight);
00093         glMaterialfv(GL_FRONT, GL_SPECULAR, specular);
00094         glMaterialfv(GL_FRONT, GL_SHININESS, shininess);
00095         //glLightModelfv(GL_FRONT, GL_SHININESS, specular);
00096         glLightfv(GL_LIGHT0, GL_POSITION, lightPosition);
00097         glLightfv(GL_LIGHT0, GL_DIFFUSE, whiteLight);
00098         glLightfv(GL_LIGHT0, GL_SPECULAR, whiteLight);
00099         glClearColor(0.0, 0.0, 0.0, 0.0);
00100         glShadeModel(GL_SMOOTH);
00101         
00102         glEnable(GL_LIGHTING);
00103         glEnable(GL_LIGHT0);
00104         //glEnable(GL_DEPTH_TEST);
00105 }
00106 
00107 // Drawn Ground
00108 
00109 void DrawGround(void) {
00110         GLfloat fExtent = 20.0;
00111         GLfloat fStep = 1.0f;
00112         GLint y = -2.4f;// Ground level 
00113         GLint iLine;
00114         glBegin(GL_LINES);
00115                 for(iLine= -fExtent; iLine <= fExtent; iLine += fStep) {
00116                         // Draw z lines
00117                         glVertex3f(iLine, y, fExtent);
00118                         glVertex3f(iLine, y, -fExtent);
00119                         // Draw x lines
00120                         glVertex3f(fExtent, y, iLine);
00121                         glVertex3f(-fExtent, y, iLine);
00122                 }
00123         glEnd();
00124 }
00125                         
00126 void display(void) {
00127         glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
00128         glColor3f(1.0, 1.0, 1.0);
00129         
00130         glPushMatrix();
00131         //gluLookAt(0, 0, 5, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0);
00132         glTranslatef(0, 0, -5);
00133         glMultTransposeMatrixf(rot45);
00134         glutSolidCube(1);
00135         //glMatrixMode(GL_MODELVIEW);
00136         
00137         glPopMatrix();
00138         
00139 
00140         //glTranslatef(0.0, 0.0, -5.0);
00141         
00142         //glRectf(-25.0, -25.0, 25.0, 25.0);
00143         glLoadIdentity();
00144         gluLookAt(0, 0, 8, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0);
00145         glRotatef(-phi, 0, 0, 1);
00146         glRotatef(theta, 0, 1, 0);
00147         //glTranslatef(0, -5.0f, 0);            
00148         
00149 
00150         
00151         DrawGround();
00152         glutSwapBuffers();
00153 }
00154 
00155 void reshape(int w, int h) {
00156         GLfloat fAspect;
00157         // Prevent a divide by zero, when window is too short
00158         if(h == 0)
00159                 h = 1;
00160         glViewport(0, 0, w, h);
00161         fAspect = (GLfloat)w / (GLfloat)h;
00162         glMatrixMode(GL_PROJECTION);
00163         // Reset the coordinate system
00164         glLoadIdentity();
00165         // Set the clipping volume
00166         gluPerspective(60.0f, fAspect, 1.0f, 50.0f);
00167         glMatrixMode(GL_MODELVIEW);
00168         glLoadIdentity();
00169 }
00170 
00171 void spin(void) {
00172         //angle += 90;
00173         //if (angle > 360) angle = angle -360;
00174         glutPostRedisplay();
00175 }
00176 
00177 void mouse(int x, int y) {
00178         // Capture the keyboard button
00179         keyboard = glutGetModifiers();
00180         // If both a left mouse botton and ALT key are pressed then do
00181         if  (keyboard == GLUT_ACTIVE_CTRL) {
00182                         theta += (x - last_x)*0.25f; 
00183                         phi += (y - last_y)*0.25f;
00184                         if (phi < -20.0f) phi = -20.0f;
00185                         else if (phi > 80.0f) phi = 80;
00186                         last_x = x;
00187                         last_y = y;
00188         }
00189 }
00190 
00191 void keyb(unsigned char key, int x, int y) {
00192         switch(key) {
00193                 case 'd' :
00194                         theta += (x - last_x)*0.0025f; 
00195                         glutPostRedisplay();
00196                         break;
00197                 case 'D' :
00198                         phi += (y - last_y)*0.0025f;
00199                         if (phi < -20.0f) phi = -20.0f;
00200                         else if (phi > 80.0f) phi = 80;
00201                         last_x = x;
00202                         last_y = y;
00203                         glutPostRedisplay();
00204                         break;
00205                 case 'y' :
00206                         //year= (year +5) % 360;
00207                         glutPostRedisplay();
00208                         break;
00209                 case 'Y' :
00210                         //year= (year -5) % 360;
00211                         glutPostRedisplay();
00212                         break;
00213         }
00214 }       
00215 
00216 class OpenGLThread: public QThread
00217         {
00218         public:
00219                 OpenGLThread(int argc, char **argv, QObject * parent = 0):QThread(parent), argc(argc), argv(argv)       { }
00220 
00221                 void run()
00222                         {
00223                         glutInit(&argc, argv);
00224                         glutInitDisplayMode(GLUT_DOUBLE | GLUT_RGB);
00225                         glutInitWindowSize(800, 600);
00226                         //glutInitWindowPosition(100,100);
00227                         // Create the window
00228                         glutCreateWindow(argv[0]);
00229                         // Initialize rendering
00230                         init();
00231                         // Whenever GLUT determines that the contents of the window needs to be redisplayed,
00232                         // the callback function registered by glutDisplayFunc() is executed
00233                         glutDisplayFunc(display);
00234                         // Determines what action should be taken when the windows is resized
00235                         glutReshapeFunc(reshape);
00236                         // Determines the action when the mouse is moved or a botton is pressed
00237                         glutKeyboardFunc(keyb);
00238                         glutMainLoop();
00239                         }
00240         private:
00241                 int argc;
00242                 char **argv;
00243         };
00244 
00246 
00247 void GetHotPoints(const QVImage<sFloat> cornerResponseImage, QList<QPoint> &hotPoints, uInt maxWindow)
00248         {
00249         const uInt rows = cornerResponseImage.getRows(), cols = cornerResponseImage.getCols();
00250 
00251         QVImage<uChar> binaryCornerImage(cols, rows);
00252         FilterLocalMax(cornerResponseImage, binaryCornerImage, maxWindow, maxWindow);
00253 
00254         QVIMAGE_INIT_READ(uChar,binaryCornerImage);
00255         for (uInt row = 0; row < binaryCornerImage.getRows(); row++)
00256                 for (uInt col = 0; col < binaryCornerImage.getCols(); col++)
00257                         if (QVIMAGE_PIXEL(binaryCornerImage, col, row,0))
00258                                 hotPoints.append(QPoint(col, row));
00259         }
00260 
00261 void GetMaximalPoints(const QVImage<sFloat> cornerResponseImage, QList<QPoint> &hotPoints, QList<QPoint> &maximalPoints, uInt maxPoints)
00262         {
00263         while( hotPoints.size() > 0 && maximalPoints.size() < maxPoints )
00264                 {
00265                 uInt maxIndex = 0;
00266                 for (int n=0; n < hotPoints.size(); n++)
00267                         if ( cornerResponseImage(hotPoints.at(n)) >  cornerResponseImage(hotPoints.at(maxIndex)) )
00268                                 maxIndex = n;
00269 
00270                 maximalPoints.append(hotPoints.at(maxIndex));
00271                 hotPoints.removeAt(maxIndex);
00272                 }
00273         }
00274 
00275 uInt    getClosestPointIndex(const QPoint point, const QList<QPoint> &pointList)
00276         {
00277         uInt index = 0;
00278         for (uInt n = 1; n < pointList.size(); n++)
00279                 if ((point - pointList.at(n)).manhattanLength() < (point - pointList.at(index)).manhattanLength())
00280                         index = n;
00281 
00282         return index;
00283         }
00284 
00285 QPoint  getMeanPoint(const QList<QPoint> &pointList)
00286         {
00287         QPoint center(0,0);
00288 
00289         for (uInt n = 0; n < pointList.size(); n++)
00290                 {
00291                 center.rx() += pointList.at(n).x();
00292                 center.ry() += pointList.at(n).y();
00293                 }
00294 
00295         center.rx() = center.rx() / pointList.size();
00296         center.ry() = center.ry() / pointList.size();
00297 
00298         return center;
00299         }
00300 
00301 double  angle(const QPoint &p)
00302         {
00303         double x = p.x(), y = p.y();
00304         if (x>0)
00305                 if (y>=0)
00306                         return atan(y/x);
00307                 else
00308                         return atan(y/x) + 2*PI;
00309         else if (x == 0)
00310                 if (y>0)
00311                         return PI/2;
00312                 else
00313                         return 3*PI/2;
00314         else // x < 0
00315                 return atan(y/x)+PI;
00316         }
00317 
00318 double  clockWiseAngle(const QPoint &p1, const QPoint &p2)
00319         {
00320         double clockAngle = angle(p2) - angle(p1);
00321         return (clockAngle < 0)? clockAngle + 2*PI:clockAngle;
00322         }
00323 
00324 // This is suposed to sort the points in a list, by... who knows?
00325 // Anyway, it doesn't works
00326 bool SortTemplatePoints(QList<QPoint> &points)
00327         {
00328         if (points.size() != 5)
00329                 return false;
00330 
00331         QList<QPoint> result;
00332 
00333         // This array will keep the index of the points.
00334         uInt index[5];
00335 
00336         // Calculate the index of the central point
00337         uInt indexp = getClosestPointIndex(getMeanPoint(points), points);
00338 
00339         result.append(points.at(indexp));
00340         points.removeAt(indexp);
00341 
00342         // Calculate the index of the top left point
00343         double minDistance = 1000000;
00344         for (uInt n = 0; n < points.size(); n++)
00345                 if ( points.at(n).manhattanLength() < minDistance )
00346                         {
00347                         minDistance = points.at(n).manhattanLength();
00348                         indexp = n;
00349                         }
00350 
00351         result.append(points.at(indexp));
00352         points.removeAt(indexp);
00353 
00354         // Calculate the other points
00355         while(points.size() > 0)
00356                 {
00357                 indexp = 0;
00358                 double minAngle = clockWiseAngle( result.back() - result.front(), points.at(indexp) - result.front());
00359         
00360                 for (uInt n = 1; n < points.size(); n++)
00361                         {
00362                         double actualAngle = clockWiseAngle( result.back() - result.front(), points.at(n) - result.front());
00363                         if ( actualAngle < minAngle )
00364                                 {
00365                                 minAngle = actualAngle;
00366                                 indexp = n;
00367                                 }
00368                         }
00369 
00370                 result.append(points.at(indexp));
00371                 points.removeAt(indexp);
00372                 }
00373 
00374         points = result;
00375 
00376         return true;
00377         }
00378 
00379 // This function returns the homography that maps the points from a source position to a
00380 // destination position
00381 Matrix<> CalibrateHomography(const Matrix<> &sourcePointsMatrix, const Matrix<> &destinationPoints)
00382         {
00383         Q_ASSERT(sourcePointsMatrix.num_cols() == 2);
00384         Q_ASSERT(sourcePointsMatrix.num_cols() == destinationPoints.num_cols());
00385         Q_ASSERT(sourcePointsMatrix.num_rows() == destinationPoints.num_rows());
00386 
00387         const uInt rows = sourcePointsMatrix.num_rows();
00388 
00389         // First, state the coefs matrix for the homogeneous linear system of equations, of the form:
00390         //      Ax = 0
00391         Matrix<> coefsMatrix(3*rows,9);
00392 
00393         for (uInt n = 0; n < rows; n++)
00394                 {
00395                 double  x = sourcePointsMatrix[n][0], y = sourcePointsMatrix[n][1],
00396                         p = destinationPoints[n][0], q = destinationPoints[n][1];
00397 
00398                 double  equation1[9] = {        0,      0,      0,      -x,     -y,     -1,     q*x,    q*y,    q},
00399                         equation2[9] = {        x,      y,      1,      0,      0,      0,      -p*x,   -p*y,   -p},
00400                         equation3[9] = {        -q*x,   -q*y,   -q,     p*x,    p*y,    p,      0,      0,      0};
00401 
00402                 coefsMatrix[3*n] = Vector<9>(equation1);
00403                 coefsMatrix[3*n+1] = Vector<9>(equation2);
00404                 coefsMatrix[3*n+2] = Vector<9>(equation3);
00405                 }
00406 
00407         // create the SVD of M
00408         SVD<> svdCoefsMatrix(coefsMatrix);
00409 
00410         Vector<9> x =  svdCoefsMatrix.get_VT()[8];
00411         Matrix<> homography(3,3);
00412 
00413         homography[0][0] = x[0];        homography[0][1] = x[1];        homography[0][2] = x[2];
00414         homography[1][0] = x[3];        homography[1][1] = x[4];        homography[1][2] = x[5];
00415         homography[2][0] = x[6];        homography[2][1] = x[7];        homography[2][2] = x[8];
00416 
00417         return homography;
00418         }
00419 
00420 void normalizeHomogeneousCoordinates(Matrix<> &points)
00421         {
00422         const uInt cols = points.num_cols(), rows = points.num_rows();
00423 
00424         for (uInt i = 0; i < rows; i++)
00425                 for (uInt j = 0; j < cols; j++)
00426                         points[i][j] /= points[i][cols-1];
00427         }
00428 
00429 void normalizeHomogeneousCoordinates(Matrix<1,3> &points)
00430         {
00431         const uInt cols = points.num_cols(), rows = points.num_rows();
00432 
00433         for (uInt i = 0; i < rows; i++)
00434                 for (uInt j = 0; j < cols; j++)
00435                         points[i][j] /= points[i][cols-1];
00436         }
00437 
00438 // This function retunrs an integer value that indicates how well the homography maps the source points
00439 // in a list, to the points in the destination list.
00440 double  testErrorHomography(const Matrix<> &sourcePointsMatrix, const Matrix<> &destinationPoints, const Matrix<> homography)
00441         {
00442         const uInt cols = sourcePointsMatrix.num_cols(), rows = sourcePointsMatrix.num_rows();
00443 
00444         Matrix <> projectedPoints(sourcePointsMatrix.num_rows(),3);
00445         Matrix <> residuals (sourcePointsMatrix.num_rows(),3);
00446 
00447         projectedPoints = sourcePointsMatrix * homography.T();
00448         normalizeHomogeneousCoordinates(projectedPoints);
00449         residuals = projectedPoints - destinationPoints;
00450 
00451         double accum = 0;
00452         for (uInt i = 0; i < rows; i++)
00453                 {
00454                 double square = 0;
00455                 for (uInt j = 0; j < cols; j++)
00456                         square += residuals[i][j]*residuals[i][j];
00457                 accum += sqrt(square);
00458                 }
00459 
00460         return accum;
00461         }
00462 
00463 // This function creates a list of the template points, first with the center point,
00464 // then the rest of the points following a clockwise order around the center point.
00465 Matrix<> GetTemplateMatrixPoints()
00466         {
00467         //                                      Center          Norwest         Noreast         Southeast       Southwest
00468         return Matrix<5,3> ((double[5][3]){     0,0,1,          -1,+1,+1,       +1,+1,+1,       +1,-1,+1,       -1,-1,+1});
00469         }
00470 
00471 // This function sustitudes warpPerspective from the qvipp, and applies the planar transformation H
00472 // to a source image, storing the transformed image in the destination image
00473 void    myWarpPerspective(const QVImage<uChar> &src, QVImage<uChar> &dest, const Matrix <> H, const double zoom)
00474         {
00475         const double cols = src.getCols(), rows = src.getRows();
00476         const Matrix<> Hinv = SVD<>(H).get_pinv();
00477 
00478         for (double col = 0; col < cols; col++)
00479                 for (double row = 0; row < rows; row++)
00480                         {
00481                         const double x = col, y = row;
00482 
00483                         double  v[3] = { 2*(x - cols/2)/cols, -2*(y - rows/2)/cols, 1 };
00484 
00485                         Vector <3> vP;
00486                         vP = H * Vector<3>(v);
00487                         const double x0 = vP[0]/vP[2], y0 = vP[1]/vP[2];
00488                         const QPoint p2 =  QPoint( (uInt) x, (uInt) y ), p1 = QPoint( zoom*x0 + cols/2, -zoom*y0 + rows/2 );
00489 
00490                         if (dest.getROI().contains(p2) && src.getROI().contains(p1))
00491                                 dest(p1) = src(p2);
00492                         }
00493         }
00494 
00495 // This function transforms a list of n points, to a nx3 matrix, containing the rows the
00496 // homogeneous coordinates for each point in the list.
00497 void pointListToMatrix(const QVImage<uChar> &image, const QList<QPoint> &points, Matrix<> &matrix)
00498         {
00499         Q_ASSERT(points.size() == matrix.get_rows());
00500         Q_ASSERT(3 == matrix.get_cols());
00501         const double    rows = image.getRows(), cols = image.getCols();
00502 
00503         for (uInt n = 0; n < points.size(); n++)
00504                 {
00505                 const double p = points.at(n).x(), q = points.at(n).y();
00506                 double  v[3] = { 2*(p - cols/2)/cols, -2*(q - rows/2)/cols, 1 };
00507                 matrix[n] = Vector<3>(v);
00508                 }
00509         }
00510 
00511 // Gets the minimum point of the parabol that contains the three points (x1,y1), (x2,y2), (x3,y3). In Mathematica:
00512 // Solve[(2a x + b /. Solve[{   a x1^2 + b  x1 + c == y1,
00513 //                              a x2^2 + b  x2 + c == y2,
00514 //                              a x3^2 + b  x3 + c == y3        },
00515 //      {a, b, c}]) == 0, x]
00516 double minimumAtCuadraticInterpolation(const double x1, const double y1, const double x2, const double y2, const double x3, const double y3)
00517         {
00518         const double    numerator = x2*x2*y1 - x3*x3*y1 - x1*x1*y2 + x3*x3*y2 + x1*x1*y3 - x2*x2*y3,
00519                         denominator =  x2*y1 - x3*y1 - x1*y2 + x3*y2 + x1*y3 - x2*y3;
00520 
00521         return numerator/(2*denominator);
00522         }
00523 
00524 #define POW2(X) ((X)*(X))
00525 
00526 
00527 // Error function, given an 'f', and a 'H' matrix. In Mathematica:
00528 //      K = {{f, 0, 0}, {0, f, 0}, {0, 0, 1}};
00529 //      Hvar = {{h1, h2, h3}, {h4, h5, h6}, {h7, h8, h9}};
00530 //      ErrorMat[H_, K_] := Transpose[H].Inverse[K].Inverse[K].H;
00531 //      Error [ErrorM_] := ((ErrorM[[1, 1]] - ErrorM[[2, 2]])^2 + ErrorM[[1, 2]]^ 2) / ErrorM[[1, 1]]^ 2;
00532 //      ErrorFunction[H_, K_] := Error[ErrorMat[H, K]];
00533 //      ErrorFunction[Hvar, K] // Simplify
00534 #define ERROR(f)                                                                \
00535         (       (                                                               \
00536                 POW2( h1*h2 + h4*h5 + (f)*(f)*h7*h8 ) +                         \
00537                 POW2( h1*h1 - h2*h2 + h4*h4 - h5*h5 + (f)*(f)*(h7*h7-h8*h8) )   \
00538                 ) / POW2(h1*h1 + h4*h4 + (f)*(f)*h7*h7)                         \
00539         )
00540 double errorHomographyFocalDistance(const Matrix <3,3> &H, const double f)
00541         {
00542         const double    h1 = H[0][0],   h2 = H[0][1],   h3 = H[0][2],
00543                         h4 = H[1][0],   h5 = H[1][1],   h6 = H[1][2],
00544                         h7 = H[2][0],   h8 = H[2][1],   h9 = H[2][2];
00545         return ERROR(f);
00546         }
00547 
00548 double errorHomographiesFocalDistance(const QVector< Matrix <3,3> > &Hlist, const double f)
00549         {
00550         double error = 0;
00551         for (uInt n = 0; n< Hlist.size(); n++)
00552                 {
00553                 Matrix<3,3> H = Hlist[n];
00554                 const double    h1 = H[0][0],   h2 = H[0][1],   h3 = H[0][2],
00555                                 h4 = H[1][0],   h5 = H[1][1],   h6 = H[1][2],
00556                                 h7 = H[2][0],   h8 = H[2][1],   h9 = H[2][2];
00557                 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) );
00558                 }
00559 
00560         return error;
00561         }
00562 
00563 // This function finds the fov that minimices errorHomographyFocalDistance for a given homography H,
00564 // in a neighbourhood of a fov value passed in the variable 'startingFov'.
00565 double calibrateFovDistance(const Matrix<> &Horig, const double startingFov = 3)
00566         {
00567         Matrix <3,3> H;
00568 
00569         // We will work with the inverse of Horig
00570         H = SVD<>(Horig).get_pinv();
00571         double f = startingFov;
00572         
00573         for (uInt i = 0; i < 10; i++)
00574                 {
00575                 f = minimumAtCuadraticInterpolation(
00576                                         f, errorHomographyFocalDistance(H,f),
00577                                         f - 0.1, errorHomographyFocalDistance(H,f - 0.1),
00578                                         f + 0.1, errorHomographyFocalDistance(H,f + 0.1)
00579                                         );
00580                 }
00581         //std::cout << i << " f = " << f << ", ERROR = " << errorHomographyFocalDistance(H,f) << std::endl;
00582         return f;
00583         }
00584 
00585 bool minimizeFovErrorFunction(const QVector< Matrix<3,3> > &H, double &f, const uInt maxSteps = 10)
00586         {
00587         double startError = errorHomographiesFocalDistance(H,f);
00588         double lastF = f, lastError = startError;
00589 
00590         for (uInt i = 0; i < maxSteps; i++)
00591                 {
00592                 f = minimumAtCuadraticInterpolation(
00593                                         f, errorHomographiesFocalDistance(H,f),
00594                                         f - 0.1, errorHomographiesFocalDistance(H,f - 0.1),
00595                                         f + 0.1, errorHomographiesFocalDistance(H,f + 0.1)
00596                                         );
00597 
00598                 const double error = errorHomographiesFocalDistance(H,f);
00599 
00600                 //std::cout << i << " f = " << f << ", ERROR = " << error << std::endl;
00601 
00602                 if ( error < 0 || error > startError)
00603                         return false;
00604                 if (error < 0)
00605                         return false;
00606 
00607                 if (error > lastError)
00608                         {
00609                         f = lastF;
00610                         return true;
00611                         }
00612 
00613                 lastF = f;
00614                 lastError = error;
00615                 }
00616 
00617         return false;
00618         }
00619 
00620 double calibrateFovDistance(const QVector< Matrix<3,3> > &Horig, const uInt maxSteps = 10)
00621         {
00622         QVector< Matrix<3,3> > Hlist;
00623         Hlist.resize(Horig.size());
00624 
00625         
00626         // We will work with the inverse of Horig's
00627         for(uInt n=0; n< Horig.size(); n++)
00628                 Hlist[n] = SVD<>(Horig[n]).get_pinv();
00629 
00630         for (double f0 = 0.5; f0 < 10; f0 += 0.5)
00631                 {
00632                 double f = f0;
00633                 if (minimizeFovErrorFunction(Hlist, f, maxSteps))
00634                         return f;
00635                 }
00636 
00637         return 0.0;
00638         }
00639 
00640 void crossProduct(const double x1, const double y1, const double z1, const double x2, const double y2, const double z2, double &x, double &y, double &z)
00641         {
00642         x = -y2*z1 + y1*z2;
00643         y = x2*z1 - x1*z2;
00644         z = -x2*y1 + x1*y2;
00645         };
00646 
00647 void crossProduct(const Vector<3> v1, const Vector<3> v2, Vector<3> &v)
00648         {
00649         const double    x1 = v1[0], y1 = v1[1], z1 = v1[2],
00650                         x2 = v2[0], y2 = v2[1], z2 = v2[2];
00651 
00652         v[0] = -y2*z1 + y1*z2;
00653         v[1] = x2*z1 - x1*z2;
00654         v[2] = -x2*y1 + x1*y2;
00655         };
00656 
00657 
00658 /*void decomposeHomography(const double &f, const Matrix<3,3> &Horig)
00659         {
00660         const double m[9] = {1/f,0,0,0,1/f,0,0,0,1};
00661         Matrix<3,3> K(m);
00662         Matrix<3,3> HE,H;
00663 
00664         H = SVD<>(Horig).get_pinv();
00665         HE = K*H;
00666 
00667         Vector<3> R1, R2, R3, T;
00668 
00669         R1 = HE.T()[0];
00670         R2 = HE.T()[1];
00671         T = HE.T()[2];
00672 
00673         crossProduct(R1, R2, R3);
00674         //std::cout << "R1 = " << R1 << std::endl;
00675         //std::cout << "R2 = " << R2 << std::endl;
00676         //std::cout << "R3 = " << R3 << std::endl;
00677 
00678         Matrix<3,3> R, Rinv;
00679 
00680         R.T()[0] = R1;
00681         R.T()[1] = R2;
00682         R.T()[2] = R3;
00683 
00684         Rinv = SVD<>(R).get_pinv();
00685 
00686         const double    normR1 = sqrt(R1[0]*R1[0] + R1[1]*R1[1] + R1[2]*R1[2]),
00687                         normR2 = sqrt(R2[0]*R2[0] + R2[1]*R2[1] + R2[2]*R2[2]),
00688                         normR3 = sqrt(R3[0]*R3[0] + R3[1]*R3[1] + R3[2]*R3[2]);
00689                         
00690         R3[0] = sqrt((normR1+normR2)/2)* R3[0] /normR3;
00691         R3[1] = sqrt((normR1+normR2)/2)* R3[1] /normR3;
00692         R3[2] = sqrt((normR1+normR2)/2)* R3[2] /normR3;
00693 
00694         Matrix<3,4> F;
00695         F.T()[0] = R1;
00696         F.T()[1] = R2;
00697         F.T()[2] = R3;
00698         F.T()[3] = T;
00699 
00700         Vector<3> C;
00701 
00702         C = (-1) * Rinv.T() * T;
00703 
00704         std::cout << "C = " << std::endl << C << std::endl;
00705         std::cout << " R1xR1 = " << (R1[0]*R1[0] + R1[1]*R1[1] + R1[2]*R1[2]) << std::endl;
00706         std::cout << " R2xR2 = " << (R2[0]*R2[0] + R2[1]*R2[1] + R2[2]*R2[2]) << std::endl;
00707         std::cout << " R3xR3 = " << (R3[0]*R3[0] + R3[1]*R3[1] + R3[2]*R3[2]) << std::endl;
00708 
00709         std::cout << " R1xR2 = " << (R1[0]*R2[0] + R1[1]*R2[1] + R1[2]*R2[2]) << std::endl;
00710         std::cout << " R1xR3 = " << (R1[0]*R3[0] + R1[1]*R3[1] + R1[2]*R3[2]) << std::endl;
00711         std::cout << " R2xR3 = " << (R2[0]*R3[0] + R2[1]*R3[1] + R2[2]*R3[2]) << std::endl;
00712         std::cout << " CxC = " << (C[0]*C[0] + C[1]*C[1] + C[2]*C[2]) << std::endl;
00713 
00714         std::cout << "Coors = (" << C[0]/C[2] << ", " << C[1]/C[2] << ")" << std::endl;
00715 
00716         //std::cout << " distance = " << (t[0]*t[0] + t[1]*t[1] + t[2]*t[2]) << std::endl;
00717         
00718         }*/
00719 
00720 #define SCALAR_PRODUCT(V1, V2)  sqrt((V1)[0]*(V2)[0] + (V1)[1]*(V2)[1] + (V1)[2]*(V2)[2])
00721 #define ABS(X)  ((X>0)?(X):(-X))
00722 void decomposeHomography(const double &f, const Matrix<3,3> &Horig, double &x, double &y, double &alpha, double &beta, double &gamma)
00723         {
00724         // O se coge H -> filas, o se coge Hinversa -> columnas
00725         // Valor positivo de f, valor negativo de f
00726         const double m[9] = {1/ABS(f),0,0,0,1/ABS(f),0,0,0,1};
00727         Matrix<3,3> Kinv(m);
00728         Matrix<3,3> R_Rt,H;
00729 
00730         //H = Horig;
00731         H = SVD<>(Horig).get_pinv();
00732 
00733         // Esta matriz es la que corresponde al segundo multiplicando del lado derecho de la expresión:
00734         //
00735         //      H = K[R|-Rt]
00736         //
00737         R_Rt = Kinv*H;
00738 
00739         Vector<3> v1 = R_Rt.T()[0], v2 = R_Rt.T()[1];
00740 
00741         // Homogenize R_Rt such as the product of the matrix M by its traspose has identity in
00742         // the 3x3 top left matrix. DUDA: esto se debería hacer antes, con la matrix H, o
00743         // ahora con la matriz R_Rt
00744         R_Rt /= (SCALAR_PRODUCT(v1, v1) + SCALAR_PRODUCT(v2, v2)) / 2;
00745 
00746         Vector<3> R1, R2, R3, Rt;
00747 
00748         R1 = R_Rt.T()[0];
00749         R2 = R_Rt.T()[1];
00750         Rt = R_Rt.T()[2];
00751 
00752         crossProduct(R1, R2, R3);
00753 
00754         Matrix<3,4> M;
00755 
00756         M.T()[0] = R1;
00757         M.T()[1] = R2;
00758         M.T()[2] = R3;
00759         M.T()[3] = Rt;
00760 
00761         std::cout << "M = " << std::endl << M << std::endl;
00762 
00763         Matrix<4,4> Mult;
00764 
00765         Mult = M.T() * M;
00766 
00767         // Position is at the first tree elements of last col of the following matrix
00768         std::cout << "M^T * M = " << std::endl << Mult << std::endl;
00769         }
00770 
00771 #define MAX_BEST_MATRICES       12
00772 class MyWorker: public QVWorker
00773         {
00774         double  matrixError[MAX_BEST_MATRICES];
00775         QVector< Matrix<3,3> > matrixList;
00776 
00777         public:
00778                 MyWorker(QString name): QVWorker(name)
00779                         {
00780                         matrixList.resize(MAX_BEST_MATRICES);
00781                         for(uInt n=0; n<MAX_BEST_MATRICES; n++)
00782                                 matrixError[n] = 100;
00783 
00784                         addProperty<double>("Max error", inputFlag, 0.02, "for an homography to be considered good", 0, 0.1);
00785                         addProperty<double>("Zoom", inputFlag, 50, "Size of the rectified template", 1, 100);
00786 
00787                         addProperty<double>("Focal", outputFlag, 0, "Focal distance");
00788 
00789                         addProperty<int>("Window size", inputFlag, 10, "Corner response window search", 1, 100);
00790                         addProperty< QVImage<uChar,1> >("Input image", inputFlag|outputFlag);
00791                         addProperty< QVImage<uChar,3> >("Corners", outputFlag);
00792                         addProperty< QVImage<uChar,1> >("Wrapped", outputFlag);
00793 
00794                         addTrigger("Calibrate");
00795                         addTrigger("Locate");
00796                         addProperty< Matrix<> >("Actual homography", outputFlag);
00797                         }
00798 
00799                 void processTrigger(QString triggerName)
00800                         {
00801                         const Matrix<3,3> H = getPropertyValue< Matrix<> > ("Actual homography");
00802 
00803                         if (triggerName == "Calibrate")
00804                                 {
00805                                 /*std::cout <<  "H = {"
00806                                                 << "{" << H[0][0] << ", " << H[0][1] << ", " << H[0][2] << "},"
00807                                                 << "{" << H[1][0] << ", " << H[1][1] << ", " << H[1][2] << "},"
00808                                                 << "{" << H[2][0] << ", " << H[2][1] << ", " << H[2][2] << "}};" << std::endl;
00809         
00810                                 std::cout << "--------- Best homographies ----------" << std::endl;
00811                                 for (uInt n=0; n<MAX_BEST_MATRICES; n++)
00812                                         {
00813                                         Matrix<3,3> H;
00814                                         H = matrixList[n];
00815                                         std::cout <<    "H" << n <<" = {"
00816                                                 << "{" << H[0][0] << ", " << H[0][1] << ", " << H[0][2] << "},"
00817                                                 << "{" << H[1][0] << ", " << H[1][1] << ", " << H[1][2] << "},"
00818                                                 << "{" << H[2][0] << ", " << H[2][1] << ", " << H[2][2] << "}}; (* " << matrixError[n] << "*)" << std::endl;
00819                                         }*/
00820         
00821                                 double fov = calibrateFovDistance(matrixList);
00822                                 std::cout << "FOV[" << MAX_BEST_MATRICES << "] (more stable) = " << fov << std::endl;
00823                                 std::cout << "FOV = " << calibrateFovDistance(getPropertyValue< Matrix<> >("Actual homography"),fov+1) << std::endl;
00824                                 setPropertyValue<double>("Focal",fov);
00825                                 }
00826                         else if (triggerName == "Locate")
00827                                 {
00828                                 const double fov = getPropertyValue<double>("Focal");
00829                                 std::cout << "----------------------------" << std::endl;
00830                                 double x, y, alpha, beta, gamma;
00831                                 if (fov != 0)
00832                                         decomposeHomography(fov, H, x, y, alpha, beta, gamma);
00833                                 }
00834                         };
00835 
00836                 void iterate()
00837                         {
00838                         const QVImage<uChar> image = getPropertyValue< QVImage<uChar,1> >("Input image");
00839                         const uInt      rows = image.getRows(), cols = image.getCols(),
00840                                         sizeMax = getPropertyValue<int>("Window size");
00841 
00842                         const double    maxError = getPropertyValue<double>("Max error"),
00843                                         zoom = getPropertyValue<double>("Zoom"),
00844                                         focal = getPropertyValue<double>("Focal");
00845 
00846                         QVImage<uChar,3> destino = image;
00847 
00848                         timeFlag("grab Frame");
00849 
00851                         // Harris corner response image
00852                         QVImage<sFloat> temp(cols, rows), cornerResponseImage(cols, rows);
00853                         //HarrisCornerResponseImage(image, cornerResponseImage);
00854                         SobelCornerResponseImage(image, cornerResponseImage);
00855 
00856                         timeFlag("Corner response image");
00857 
00859                         // Hot points
00860                         QList<QPoint> hotPoints;
00861                         GetHotPoints(cornerResponseImage, hotPoints, sizeMax);
00862 
00863                         timeFlag("Get hotpoints");
00864 
00866                         // Calibración
00867                         QList<QPoint> maximalPoints;
00868                         GetMaximalPoints(cornerResponseImage, hotPoints, maximalPoints, 5);
00869                         SortTemplatePoints(maximalPoints);
00870 
00871                         drawPoints(maximalPoints, destino);
00872 
00873                         //std::cout << "----------------------------" << std::endl;
00874 
00875                         //for (uInt i=0; i< maximalPoints.size(); i++)
00876                         //      std::cout << "MaximalPoint (" << maximalPoints.at(i).x() << ", " << maximalPoints.at(i).y() << "), value = " << (double)cornerResponseImage(maximalPoints.at(i)) << std::endl;
00877 
00878                         timeFlag("Get max hotpoints");
00879 
00880                         if (maximalPoints.size() == 5)
00881                                 {
00882                                 Matrix <> sourcePointsMatrix(5,3), destinationPointsMatrix(5,3);
00883                                 destinationPointsMatrix =  GetTemplateMatrixPoints();
00884                                 pointListToMatrix(image, maximalPoints, sourcePointsMatrix);
00885 
00886                                 Matrix<> H = CalibrateHomography(sourcePointsMatrix, destinationPointsMatrix);
00887 
00888                                 const double actualError = testErrorHomography(sourcePointsMatrix, destinationPointsMatrix, H);
00889                                 if (actualError < maxError)
00890                                         {
00891                                         setPropertyValue< Matrix<> >("Actual homography", H);
00892 
00893                                         uInt index = 0;
00894                                         for (uInt i=1; i<MAX_BEST_MATRICES; i++)
00895                                                 if (matrixError[i] > matrixError[index])
00896                                                         index = i;
00897 
00898                                         if (matrixError[index] > actualError)
00899                                                 {
00900                                                 matrixList[index] = H;
00901                                                 matrixError[index] = actualError;
00902                                                 }
00903 
00904                                         QVImage<uChar> wrapped(cols, rows);
00905                                         Set(wrapped,0);
00906         
00907                                         myWarpPerspective(image, wrapped, H, zoom);
00908 
00909                                         setPropertyValue< QVImage<uChar,1> >("Wrapped", wrapped);
00910 
00911                                         }
00912                                 //std::cout << "----------------------------" << std::endl;
00913                                 //std::cout << "H = " << std::endl << H << std::endl;
00914                                 //std::cout << "Hinv = " << std::endl << SVD<>(H).get_pinv() << std::endl;
00915 
00916                                 //setPropertyValue<double>("Focal",0);
00917                                 }
00918 
00919                         timeFlag("Calibrate");
00920 
00921                         // Fin rectificación ////////////////////////
00922 
00923                         setPropertyValue< QVImage<uChar,3> >("Corners", destino);
00924                         timeFlag("Draw corners");
00925                 
00926                         }
00927         };
00928 
00929 int main(int argc, char *argv[])
00930         {
00931         QVApplication app(argc, argv,
00932                 
00933                 "Example program for QVision library. Applies corner detection over an input video."
00934 
00935                 );
00936 
00938         OpenGLThread openGLThread(argc, argv);
00939         openGLThread.start();
00940         
00942 
00943         QVMPlayerCamera camera("Video");
00944         MyWorker worker("Corners Worker");
00945         camera.link(&worker,"Input image");
00946 
00947         QVGUI interface;
00948 
00949         QVImageCanvas imageCanvas("Corners");
00950         imageCanvas.linkProperty(worker, "Corners");
00951 
00952         QVImageCanvas imageCanvas2("Wrapped");
00953         imageCanvas2.linkProperty(worker, "Wrapped");
00954 
00955         return app.exec();
00956         }
00957 
00959 

Generated on Thu Mar 13 19:18:16 2008 for QVision by  doxygen 1.5.3