00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
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>
00060 #include <TooN/numhelpers.h>
00061 #include <TooN/SVD.h>
00062 #include <TooN/LU.h>
00063 #include <TooN/SymEigen.h>
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
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
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
00093 glMaterialfv(GL_FRONT, GL_SPECULAR, specular);
00094 glMaterialfv(GL_FRONT, GL_SHININESS, shininess);
00095
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
00105 }
00106
00107
00108
00109 void DrawGround(void) {
00110 GLfloat fExtent = 20.0;
00111 GLfloat fStep = 1.0f;
00112 GLint y = -2.4f;
00113 GLint iLine;
00114 glBegin(GL_LINES);
00115 for(iLine= -fExtent; iLine <= fExtent; iLine += fStep) {
00116
00117 glVertex3f(iLine, y, fExtent);
00118 glVertex3f(iLine, y, -fExtent);
00119
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
00132 glTranslatef(0, 0, -5);
00133 glMultTransposeMatrixf(rot45);
00134 glutSolidCube(1);
00135
00136
00137 glPopMatrix();
00138
00139
00140
00141
00142
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
00148
00149
00150
00151 DrawGround();
00152 glutSwapBuffers();
00153 }
00154
00155 void reshape(int w, int h) {
00156 GLfloat fAspect;
00157
00158 if(h == 0)
00159 h = 1;
00160 glViewport(0, 0, w, h);
00161 fAspect = (GLfloat)w / (GLfloat)h;
00162 glMatrixMode(GL_PROJECTION);
00163
00164 glLoadIdentity();
00165
00166 gluPerspective(60.0f, fAspect, 1.0f, 50.0f);
00167 glMatrixMode(GL_MODELVIEW);
00168 glLoadIdentity();
00169 }
00170
00171 void spin(void) {
00172
00173
00174 glutPostRedisplay();
00175 }
00176
00177 void mouse(int x, int y) {
00178
00179 keyboard = glutGetModifiers();
00180
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
00207 glutPostRedisplay();
00208 break;
00209 case 'Y' :
00210
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
00227
00228 glutCreateWindow(argv[0]);
00229
00230 init();
00231
00232
00233 glutDisplayFunc(display);
00234
00235 glutReshapeFunc(reshape);
00236
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
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
00325
00326 bool SortTemplatePoints(QList<QPoint> &points)
00327 {
00328 if (points.size() != 5)
00329 return false;
00330
00331 QList<QPoint> result;
00332
00333
00334 uInt index[5];
00335
00336
00337 uInt indexp = getClosestPointIndex(getMeanPoint(points), points);
00338
00339 result.append(points.at(indexp));
00340 points.removeAt(indexp);
00341
00342
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
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
00380
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
00390
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
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
00439
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
00464
00465 Matrix<> GetTemplateMatrixPoints()
00466 {
00467
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
00472
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
00496
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
00512
00513
00514
00515
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
00528
00529
00530
00531
00532
00533
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
00564
00565 double calibrateFovDistance(const Matrix<> &Horig, const double startingFov = 3)
00566 {
00567 Matrix <3,3> H;
00568
00569
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
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
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
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
00659
00660
00661
00662
00663
00664
00665
00666
00667
00668
00669
00670
00671
00672
00673
00674
00675
00676
00677
00678
00679
00680
00681
00682
00683
00684
00685
00686
00687
00688
00689
00690
00691
00692
00693
00694
00695
00696
00697
00698
00699
00700
00701
00702
00703
00704
00705
00706
00707
00708
00709
00710
00711
00712
00713
00714
00715
00716
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
00725
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
00731 H = SVD<>(Horig).get_pinv();
00732
00733
00734
00735
00736
00737 R_Rt = Kinv*H;
00738
00739 Vector<3> v1 = R_Rt.T()[0], v2 = R_Rt.T()[1];
00740
00741
00742
00743
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
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
00806
00807
00808
00809
00810
00811
00812
00813
00814
00815
00816
00817
00818
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
00852 QVImage<sFloat> temp(cols, rows), cornerResponseImage(cols, rows);
00853
00854 SobelCornerResponseImage(image, cornerResponseImage);
00855
00856 timeFlag("Corner response image");
00857
00859
00860 QList<QPoint> hotPoints;
00861 GetHotPoints(cornerResponseImage, hotPoints, sizeMax);
00862
00863 timeFlag("Get hotpoints");
00864
00866
00867 QList<QPoint> maximalPoints;
00868 GetMaximalPoints(cornerResponseImage, hotPoints, maximalPoints, 5);
00869 SortTemplatePoints(maximalPoints);
00870
00871 drawPoints(maximalPoints, destino);
00872
00873
00874
00875
00876
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
00913
00914
00915
00916
00917 }
00918
00919 timeFlag("Calibrate");
00920
00921
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