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