00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021 #include <qvprojective.h>
00022 #include <qvmatrixalgebra.h>
00023 #include <qvdefines.h>
00024 #include <qvmath.h>
00025 #include <float.h>
00026 #include <qvnumericalanalysis.h>
00027
00031
00032 void homogenizePoints(const QList< QPointFMatching > &matchings, QVMatrix &premult, QVMatrix &postmult, QList< QPointFMatching > &homogeneizedPairs)
00033 {
00034
00035
00036
00037 float minXS=FLT_MAX, maxXS=-FLT_MAX, minYS=FLT_MAX, maxYS=-FLT_MAX,
00038 minXD=FLT_MAX, maxXD=-FLT_MAX, minYD=FLT_MAX, maxYD=-FLT_MAX;
00039
00040
00041 foreach(QPointFMatching matching, matchings)
00042 {
00043 minXS = MIN(matching.first.x(), minXS);
00044 maxXS = MAX(matching.first.x(), maxXS);
00045 minYS = MIN(matching.first.y(), minYS);
00046 maxYS = MAX(matching.first.y(), maxYS);
00047
00048 minXD = MIN(matching.second.x(), minXD);
00049 maxXD = MAX(matching.second.x(), maxXD);
00050 minYD = MIN(matching.second.y(), minYD);
00051 maxYD = MAX(matching.second.y(), maxYD);
00052 }
00053
00054
00055 if(fabs(minXS-maxXS) < EPSILON)
00056 maxXS += 1.0;
00057 if(fabs(minYS-maxYS) < EPSILON)
00058 maxYS += 1.0;
00059 if(fabs(minXD-maxXD) < EPSILON)
00060 maxXD += 1.0;
00061 if(fabs(minYD-maxYD) < EPSILON)
00062 maxYD += 1.0;
00063
00064 minXS = 0; maxXS = 320;
00065 minYS = 0; maxYS = 240;
00066
00067
00068 foreach(QPointFMatching matching, matchings)
00069 {
00070 const double x = (matching.first.x()-(maxXS+minXS)/2)/(maxXS-minXS),
00071 y = (matching.first.y()-(maxYS+minYS)/2)/(maxYS-minYS),
00072 x_p = (matching.second.x()-(maxXD+minXD)/2)/(maxXD-minXD),
00073 y_p = (matching.second.y()-(maxYD+minYD)/2)/(maxYD-minYD);
00074
00075 homogeneizedPairs.append(QPointFMatching(QPointF(x,y),QPointF(x_p,y_p)));
00076 }
00077
00078 const double dataPremult[9] = {
00079 1/(maxXS-minXS), 0, -(maxXS+minXS)/(2*(maxXS-minXS)),
00080 0, 1/(maxYS-minYS), -(maxYS+minYS)/(2*(maxYS-minYS)),
00081 0, 0, 1
00082 },
00083 dataPostmult[9] = {
00084 maxXD-minXD, 0, (maxXD+minXD)/2,
00085 0, maxYD-minYD, (maxYD+minYD)/2,
00086 0, 0, 1,
00087 };
00088
00089 premult = QVMatrix(3,3, dataPremult);
00090 postmult = QVMatrix(3,3, dataPostmult);
00091 }
00092
00093 QVMatrix ComputeProjectiveHomography(const QList< QPointFMatching > &matchings)
00094 {
00095 Q_ASSERT(matchings.size() >= 4);
00096
00097
00098
00099
00100 QList< QPointFMatching > homogeneizedPairs;
00101 QVMatrix premult, postmult;
00102
00103 homogenizePoints(matchings, premult, postmult, homogeneizedPairs);
00104
00105
00106
00107 QVMatrix A(3*homogeneizedPairs.size(),9);
00108 for (int n = 0; n < homogeneizedPairs.size(); n++)
00109 {
00110 const QPointFMatching matching = homogeneizedPairs.at(n);
00111 const double x = matching.first.x(),
00112 y = matching.first.y(),
00113 x_p = matching.second.x(),
00114 y_p = matching.second.y();
00115
00116 const double coefsMatrixData[3 * 9] = {
00117
00118 0, 0, 0,
00119 -x, -y, -1,
00120 x*y_p, y*y_p, y_p,
00121
00122 x, y, 1,
00123 0, 0, 0,
00124 -x*x_p, -y*x_p, -x_p,
00125
00126 -x*y_p, -y*y_p, -y_p,
00127 x*x_p, y*x_p, x_p,
00128 0, 0, 0
00129 };
00130
00131 const QVMatrix coefsMatrix(3,9, coefsMatrixData);
00132
00133 A.setRow(3*n+0, coefsMatrix.getRow(0));
00134 A.setRow(3*n+1, coefsMatrix.getRow(1));
00135 A.setRow(3*n+2, coefsMatrix.getRow(2));
00136 }
00137
00138
00139 QVVector x(9);
00140
00141 solveHomogeneousLinear(A, x);
00142
00143
00144 QVMatrix homography = QVMatrix(x.mid(0,3)) & QVMatrix(x.mid(3,3)) & QVMatrix(x.mid(6,3));
00145
00146
00147 homography = (postmult * homography) * premult;
00148 homography = homography / homography(2,2);
00149
00150 return homography;
00151 }
00152
00153 QVMatrix ComputeAffineHomography(const QList< QPointFMatching > &matchings)
00154 {
00155 Q_ASSERT(matchings.size() >= 3);
00156
00157
00158
00159
00160 QList< QPointFMatching > homogeneizedPairs;
00161 QVMatrix premult, postmult;
00162
00163 homogenizePoints(matchings, premult, postmult, homogeneizedPairs);
00164
00165
00166
00167 QVMatrix A(2*homogeneizedPairs.size(),6);
00168 QVVector b(2*homogeneizedPairs.size());
00169
00170 for (int n = 0; n < homogeneizedPairs.size(); n++)
00171 {
00172 const QPointFMatching matching = homogeneizedPairs.at(n);
00173 const double x = matching.first.x(),
00174 y = matching.first.y(),
00175 x_p = matching.second.x(),
00176 y_p = matching.second.y();
00177
00178 const double coefsMatrixData[2 * 6] = {
00179 x, y, 1, 0, 0, 0,
00180 0, 0, 0, x, y, 1
00181 };
00182
00183 const QVMatrix coefsMatrix(2,6, coefsMatrixData);
00184
00185 A.setRow(2*n+0, coefsMatrix.getRow(0));
00186 A.setRow(2*n+1, coefsMatrix.getRow(1));
00187 b[2*n+0] = x_p;
00188 b[2*n+1] = y_p;
00189 }
00190
00191
00192 const QVVector x = pseudoInverse(A)*b;
00193
00194
00195 const QVMatrix M = QVMatrix(x.mid(0,3)) & QVMatrix(x.mid(3,3)) & (QVVector(2,0.0) << 1);
00196
00197
00198 return (postmult * M) * premult;
00199 }
00200
00201 QVMatrix ComputeEuclideanHomography(const QList< QPointFMatching > &matchings)
00202 {
00203 if (matchings.size() < 2)
00204 return QVMatrix::identity(3);
00205
00206 QPointF meanSource(0.0, 0.0), meanDestination(0.0, 0.0);
00207 foreach(QPointFMatching matching, matchings)
00208 {
00209 meanSource = meanSource + matching.first;
00210 meanDestination = meanDestination + matching.second;
00211 }
00212
00213 meanSource = meanSource / double(matchings.size());
00214 meanDestination = meanDestination / double(matchings.size());
00215
00216 QVVector angles, scales;
00217 foreach(QPointFMatching matching, matchings)
00218 {
00219 const QPointF source = matching.first - meanSource,
00220 destination = matching.second - meanDestination;
00221
00222 angles << qvAngle(destination, source);
00223 scales << norm2(destination) / norm2(source);
00224 }
00225
00226 return QVMatrix::translationMatrix(meanDestination.x(), meanDestination.y()) *
00227 QVMatrix::rotationMatrix(angles.median()) * QVMatrix::scaleMatrix(scales.median()) *
00228 QVMatrix::translationMatrix(-meanSource.x(), -meanSource.y());
00229 }
00230
00231
00232
00233
00234
00235
00236
00237
00238
00239
00240
00241
00242
00243
00244
00245
00246
00247
00248
00249
00250
00251
00252
00253
00254
00255
00256
00257
00258
00259
00260
00261
00262
00263 #ifdef QVOPENCV
00264 QVMatrix cvFindFundamentalMat(const QList<QPointFMatching> &matchings)
00265 {
00266 const int point_count = matchings.size();
00267
00268 CvMat *points1 = cvCreateMat(1,point_count,CV_32FC2),
00269 *points2 = cvCreateMat(1,point_count,CV_32FC2);
00270
00271 for(int i = 0; i < point_count; i++)
00272 {
00273 points1->data.fl[i*2] = matchings[i].first.x();
00274 points1->data.fl[i*2+1] = matchings[i].first.y();
00275 points2->data.fl[i*2] = matchings[i].second.x();
00276 points2->data.fl[i*2+1] = matchings[i].second.y();
00277 }
00278
00279 CvMat *fundamental_matrix = cvCreateMat(3,3,CV_32FC1);
00280
00281 const int fm_count = cvFindFundamentalMat(points1, points2, fundamental_matrix, CV_FM_8POINT);
00282 const QVMatrix result = fundamental_matrix;
00283
00284
00285 cvReleaseMat(&points1);
00286 cvReleaseMat(&points2);
00287
00288
00289 cvReleaseMat(&fundamental_matrix);
00290
00291 return (fm_count == 1)?result:QVMatrix();
00292 }
00293 #endif
00294
00295
00296
00297
00298
00299
00300
00301
00302
00303
00304
00305
00306
00307
00308
00309
00310
00311
00312
00313
00314
00315
00316
00317
00318
00319
00320
00321
00322
00323
00324
00325
00326
00327
00328
00329
00330
00331
00332
00333
00334
00335
00336
00337
00338
00339
00340
00341
00342
00343
00344
00345
00346
00347
00348
00349
00350
00351
00352
00353
00354
00355
00356
00357
00358
00359
00360
00361
00362
00363
00364
00365
00366
00367
00368
00369
00370 #ifdef QVIPP
00371 QPointF ApplyHomography(const QVMatrix &H, const QPointF &point)
00372 {
00373 const double h11 = H(0,0), h12 = H(0,1), h13 = H(0,2),
00374 h21 = H(1,0), h22 = H(1,1), h23 = H(1,2),
00375 h31 = H(2,0), h32 = H(2,1), h33 = H(2,2),
00376 x = point.x(), y = point.y(),
00377 homogenizer = h31*x + h32*y + h33;
00378
00379 return QPointF(h11*x + h12*y + h13, h21*x + h22*y + h23)/homogenizer;
00380 }
00381
00382 QList<QPointF> ApplyHomography(const QVMatrix &homography, const QList<QPointF> &sourcePoints)
00383 {
00384 QList<QPointF> result;
00385 foreach(QPointF point, sourcePoints)
00386 result.append(ApplyHomography(homography, point));
00387 return result;
00388 }
00389
00390 QVImage<uChar, 1> ApplyHomography(const QVMatrix &homography, const QVImage<uChar, 1> &image, const int interpolation)
00391 {
00392 QVImage<uChar, 1> result(image.getCols(), image.getRows());
00393 WarpPerspective(image, result, homography);
00394 return result;
00395 }
00396
00397 QVImage<uChar, 3> ApplyHomography(const QVMatrix &homography, const QVImage<uChar, 3> &image, const int interpolation)
00398 {
00399 QVImage<uChar, 3> result(image.getCols(), image.getRows());
00400 WarpPerspective(image, result, homography);
00401 return result;
00402 }
00403 #endif
00404
00405 double HomographyTestError(const QVMatrix &homography)
00406 {
00407 const QVVector v1 = homography.getCol(0), v2 = homography.getCol(1);
00408 return ABS(v1.norm2() - v2.norm2()) / (v1.norm2() + v2.norm2())
00409 + ABS(v1 * v2) / (v1.norm2() * v2.norm2());
00410 }
00411
00413
00414
00415
00416
00417
00418
00419
00420
00421
00422
00423
00424
00425
00426
00427 void GetDirectIntrinsicCameraMatrixFromHomography(const QVMatrix &H, QVMatrix &K)
00428 {
00429 const double h1 = H(0,0), h2 = H(0,1), h4 = H(1,0), h5 = H(1,1), h7 = H(2,0), h8 = H(2,1);
00430 const double focalNumerator = + (h2*(h2 + h4) - h1*h5 + pow(h5,2))*(pow(h2,2) - h2*h4 + h5*(h1 + h5))*pow(h7,2)
00431 - (pow(h1,2) + pow(h4,2))*(h1*h2 + h4*h5)*h7*h8
00432 + (pow(h1,2) + pow(h4,2))*(pow(h1,2) - pow(h2,2) + pow(h4,2) - pow(h5,2))*pow(h8,2);
00433 const double focalDenominator = + (pow(h2,2) + pow(h5,2))*pow(h7,4)
00434 - (h1*h2 + h4*h5)*pow(h7,3)*h8
00435 - (pow(h2,2) + pow(h5,2))*pow(h7,2)*pow(h8,2)
00436 + (pow(h1,2) + pow(h4,2))*pow(h8,4);
00437 const double finv = sqrt(ABS(focalNumerator / focalDenominator))/2;
00438
00439 K = QVMatrix::identity(3) * finv;
00440 K(2,2) = 1;
00441 }
00442
00443 void CalibrateCameraFromPlanarHomography(const QVMatrix &H, QVMatrix &K, QVMatrix &Rt)
00444 {
00445
00446
00447
00448 GetDirectIntrinsicCameraMatrixFromHomography(H, K);
00449
00450
00451
00452
00453 QVMatrix R12t = pseudoInverse(K)*H;
00454
00455
00456
00457
00458 Rt = QVMatrix(4,4);
00459 R12t = R12t * 2 / (R12t.getCol(0).norm2() + R12t.getCol(1).norm2());
00460
00461
00462 Rt.setCol(0, R12t.getCol(0));
00463 Rt.setCol(1, R12t.getCol(1));
00464 Rt.setCol(2, R12t.getCol(0) ^ R12t.getCol(1));
00465 Rt.setCol(3, R12t.getCol(2));
00466 Rt(3,3) = 1;
00467 }
00468
00469
00470
00471
00472
00473
00474
00475
00476
00477
00478
00479
00480 void GetExtrinsicCameraMatrixFromHomography(const QVMatrix &K, const QVMatrix &H, QVMatrix &Rt)
00481 {
00482
00483 QVMatrix R12_t = pseudoInverse(K)*pseudoInverse(H);
00484
00485
00486
00487
00488 R12_t = R12_t * 2 / (R12_t.getCol(0).norm2() + R12_t.getCol(1).norm2());
00489
00490
00491 Rt = QVMatrix(4,4);
00492
00493 Rt.setCol(0, R12_t.getCol(0));
00494 Rt.setCol(1, R12_t.getCol(1));
00495 Rt.setCol(2, R12_t.getCol(0) ^ R12_t.getCol(1));
00496 Rt.setCol(3, R12_t.getCol(2));
00497 Rt(3,3) = 1;
00498 }
00499
00500 void GetPinholeCameraIntrinsicsFromPlanarHomography(const QVMatrix &H, QVMatrix &K, const int iterations,
00501 const double maxGradientNorm, const double step, const double tol)
00502 {
00503 class KErrorFunction: public QVFunction<QVVector, double>
00504 {
00505 private:
00506 const QVMatrix H;
00507
00508 double evaluate(const QVVector &input) const
00509 {
00510 const QVMatrix Rt = pseudoInverse(KMatrix(input))*H, errorMat = Rt.transpose() * Rt;
00511 return POW2(errorMat(0,0) -1) + POW2(errorMat(1,1) -1) + 2*POW2(errorMat(1,0));
00512 }
00513
00514 public:
00515 KErrorFunction(const QVMatrix &H): QVFunction<QVVector, double>(), H(H) { }
00516
00517 const QVMatrix KMatrix(const QVVector &input) const
00518 {
00519 QVMatrix K = QVMatrix::zeros(3,3);
00520 K(0,0) = input[0];
00521 K(1,1) = input[0];
00522 K(2,2) = input[1];
00523 return K;
00524 }
00525 } errorFunction(H);
00526
00527 QVVector x(2,1);
00528 qvGSLMinimizeFDF(errorFunction, x, VectorBFGS, iterations, maxGradientNorm, step, tol);
00529 K = errorFunction.KMatrix(x);
00530 K = K / K(2,2);
00531 }
00532
00533
00534 QList<QVMatrix> getCanonicalCameraMatricesFromEssentialMatrix(const QVMatrix &E)
00535 {
00536 QVMatrix U, V, S;
00537 SingularValueDecomposition(E, U, S, V);
00538
00539
00540 QVMatrix W(3, 3, 0.0), Z(3, 3, 0.0);
00541 W(1,0) = 1; W(0,1) = -1; W(2,2) = 1;
00542 Z(1,0) = -1; Z(0,1) = 1;
00543
00544 const QVVector u3 = U.getCol(2);
00545 const QVMatrix UWVt = U * W * V.transpose(),
00546 UWtVt = U * W.transpose() * V.transpose();
00547
00548
00549 return QList<QVMatrix>() << ( UWVt | u3 ) << ( UWtVt | u3 ) << ( UWVt | u3*(-1) ) << ( UWtVt | u3*(-1) );
00550 }
00551
00552 QVMatrix getEssentialMatrixFromCanonicalCameraMatrix(const QVMatrix &P)
00553 {
00554 return P.getCol(3).crossProductMatrix() * (QVMatrix(P.getCol(0)).transpose() | P.getCol(1) | P.getCol(2));
00555 }
00556
00557 QVMatrix getCameraMatrixFrom2D3DPointCorrespondences(const QList<QPointF> &points2d, const QList<QV3DPointF> &points3d)
00558 {
00559 const int n = points2d.size();
00560 QVMatrix A(3 * n, 12);
00561 for(int i = 0; i < n; i++)
00562 {
00563 const double xp = points2d[i].x(), yp = points2d[i].y(),
00564 x = points3d[i].x(), y = points3d[i].y(), z = points3d[i].z();
00565
00566 const double dataCoefs[3*12] = {
00567 0, 0, 0, 0, -x, -y, -z, -1, +x*yp, +y*yp, +yp*z, +yp,
00568 +x, +y, +z, +1, 0, 0, 0, 0, -x*xp, -xp*y, -xp*z, -xp,
00569 -x*yp, -y*yp, -yp*z, -yp, +x*xp, +xp*y, +xp*z, +xp, 0, 0, 0, 0
00570 };
00571
00572 const QVMatrix coefs(3, 12, dataCoefs);
00573
00574 A.setRow(3*i+0, coefs.getRow(0));
00575 A.setRow(3*i+1, coefs.getRow(1));
00576 A.setRow(3*i+2, coefs.getRow(2));
00577 }
00578
00579 QVVector vectorP;
00580 solveHomogeneousLinear(A, vectorP);
00581
00582 const QVMatrix P = QVMatrix(vectorP.mid(0,4)) & QVMatrix(vectorP.mid(4,4)) & QVMatrix(vectorP.mid(8,4));
00583
00584
00585
00586 return P * SIGN((P*(QVVector(points3d.first()) << 1))[2]);
00587 }
00588
00589
00590
00591
00592
00593
00594
00595
00596
00597
00598
00599
00600
00601
00602
00603
00604
00605
00606
00607
00608
00609
00610
00611
00612
00613
00614
00615
00616
00617
00618
00619
00620
00621 QVMatrix refineExtrinsicCameraMatrixWithQRDecomposition(const QVMatrix &P)
00622 {
00623 const QVMatrix M = P.getSubmatrix(0,2,0,2), Mt = M.transpose();
00624
00625 QVMatrix Q, R;
00626 QRDecomposition(Mt, Q, R);
00627
00628 QVMatrix I_1 = QVMatrix::identity(3);
00629
00630 I_1(0,0) = SIGN(R(0,0));
00631 I_1(1,1) = SIGN(R(1,1));
00632 I_1(2,2) = SIGN(R(2,2));
00633
00634 return pseudoInverse(R.transpose()*I_1) * P;
00635 }
00636
00637 QVMatrix refineExtrinsicCameraMatrixWithPolarDecomposition(const QVMatrix &P)
00638 {
00639 const QVMatrix M = P.getSubmatrix(0,2,0,2), Mt = M.transpose();
00640
00641 QVMatrix W, S, V;
00642 SingularValueDecomposition(Mt, W, S, V);
00643
00644
00645
00646
00647
00648
00649
00650
00651
00652 const QVMatrix
00653 Pp = V * S * V.transpose();
00654
00655 QVMatrix I_Pp = QVMatrix::identity(3);
00656
00657 I_Pp(0,0) = SIGN(Pp(0,0));
00658 I_Pp(1,1) = SIGN(Pp(1,1));
00659 I_Pp(2,2) = SIGN(Pp(2,2));
00660
00661 return pseudoInverse(Pp.transpose()*I_Pp) * P;
00662 }
00663
00664 QV3DPointF triangulate3DPointFrom2Views(const QPointF &point1, const QVMatrix &P1, const QPointF &point2, const QVMatrix &P2)
00665 {
00666 QVMatrix A(4,4);
00667
00668 A.setRow(0, P1.getRow(2) * point1.x() - P1.getRow(0));
00669 A.setRow(1, P1.getRow(2) * point1.y() - P1.getRow(1));
00670 A.setRow(2, P2.getRow(2) * point2.x() - P2.getRow(0));
00671 A.setRow(3, P2.getRow(2) * point2.y() - P2.getRow(1));
00672
00673 QVVector x;
00674
00675 solveHomogeneousLinear(A, x);
00676
00677
00678
00679
00680 return QV3DPointF(x[0] / x[3], x[1] / x[3], x[2] / x[3]);
00681
00682
00683 }
00684
00685 QV3DPointF triangulate3DPointFromNViews(const QList<QPointF> &points, const QList<QVMatrix> &Plist)
00686 {
00687 QVMatrix A(2 * points.size(),4);
00688 for(int i = 0; i < points.size(); i++)
00689 {
00690 A.setRow(2*i + 0, Plist[i].getRow(2) * points[i].x() - Plist[i].getRow(0));
00691 A.setRow(2*i + 1, Plist[i].getRow(2) * points[i].y() - Plist[i].getRow(1));
00692
00693
00694
00695
00696
00697 }
00698
00699 QVVector x;
00700
00701 solveHomogeneousLinear(A, x);
00702
00703
00704
00705
00706 return QV3DPointF(x[0] / x[3], x[1] / x[3], x[2] / x[3]);
00707
00708
00709 }
00710