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 result(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 result.setRow(3*n+0, coefsMatrix.getRow(0));
00134 result.setRow(3*n+1, coefsMatrix.getRow(1));
00135 result.setRow(3*n+2, coefsMatrix.getRow(2));
00136 }
00137
00138
00139 QVVector x(9);
00140
00141 solveHomogeneousLinear(result, 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 cvFindFundamentalMat(const QList<QPointFMatching> &matchings)
00154 #ifdef OPENCV
00155 {
00156 const int point_count = matchings.size();
00157
00158 CvMat *points1 = cvCreateMat(1,point_count,CV_32FC2),
00159 *points2 = cvCreateMat(1,point_count,CV_32FC2);
00160
00161 for(int i = 0; i < point_count; i++)
00162 {
00163 points1->data.fl[i*2] = matchings[i].first.x();
00164 points1->data.fl[i*2+1] = matchings[i].first.y();
00165 points2->data.fl[i*2] = matchings[i].second.x();
00166 points2->data.fl[i*2+1] = matchings[i].second.y();
00167 }
00168
00169 CvMat *fundamental_matrix = cvCreateMat(3,3,CV_32FC1);
00170
00171 const int fm_count = cvFindFundamentalMat(points1, points2, fundamental_matrix, CV_FM_8POINT);
00172 const QVMatrix result = fundamental_matrix;
00173
00174
00175 cvReleaseMat(&points1);
00176 cvReleaseMat(&points2);
00177
00178
00179 cvReleaseMat(&fundamental_matrix);
00180
00181 return (fm_count == 1)?result:QVMatrix();
00182 }
00183 #else
00184 {
00185 std::cout << "ERROR at cvFindFundamentalMat(const QList<QPointFMatching> &matchings): requires OPENCV" << std::endl;
00186 exit(0);
00187 }
00188 #endif
00189
00190 void getMeanDirection(const QVMatrix m, QVVector &mean, QVVector &direction)
00191 {
00192 mean = m.meanCol();
00193 QVMatrix centeredM = m;
00194 for (int i = 0; i < centeredM.getCols(); i++)
00195 centeredM.setCol(i, centeredM.getCol(i) - mean);
00196
00197
00198 QVMatrix eigVecs;
00199 QVVector eigVals;
00200 eigenDecomposition(centeredM * centeredM.transpose(), eigVals, eigVecs);
00201
00202 direction = QVVector(eigVecs.getCols());
00203 for (int i = 0; i < eigVecs.getCols(); i++)
00204 direction[i] = eigVecs(0,i);
00205 direction = direction * eigVals[0];
00206 }
00207
00208
00209
00210
00211
00212
00213
00214
00215
00216
00217
00220
00221
00222
00223
00224
00225
00226
00227
00228
00229 QVMatrix ComputeEuclideanHomography(const QList< QPointFMatching > &matchings)
00230 {
00231
00232 QList<QPointF> sourcePointList, destPointList;
00233
00234 foreach(QPointFMatching matching, matchings)
00235 {
00236 sourcePointList.append(matching.first);
00237 destPointList.append(matching.second);
00238 }
00239
00240 const QVMatrix sources = sourcePointList, destinations = destPointList;
00241
00242 QVVector sourcesMean, destinationsMean, sourcesDirection, destinationsDirection;
00243 getMeanDirection(sources.transpose(), sourcesMean, sourcesDirection);
00244 getMeanDirection(destinations.transpose(), destinationsMean, destinationsDirection);
00245
00246 const QPointF C1 = sourcesMean, C2 = destinationsMean, D1 = sourcesDirection, D2 = destinationsDirection;
00247
00248
00249 double zoom = 0;
00250 int switchD1Direction = 0, zoomCount = 0;
00251 for(int i = 0; i < sourcePointList.size(); i ++)
00252 {
00253 const QPointF sourceCenteredPoint = sourcePointList.at(i) - sourcesMean, destCenteredPoint = destPointList.at(i) - destinationsMean;
00254
00255 if (norm2(sourceCenteredPoint) > 1e-10)
00256 {
00257 zoom += norm2(destCenteredPoint) / norm2(sourceCenteredPoint);
00258 zoomCount ++;
00259 }
00260
00261 if ( (norm2(destCenteredPoint - D2) - norm2(destCenteredPoint + D2)) * (norm2(sourceCenteredPoint + D1)
00262 - norm2(sourceCenteredPoint - D1)) > 0 ||
00263 (norm2(destCenteredPoint - D2) - norm2(destCenteredPoint + D2)) * (norm2(sourceCenteredPoint + D1)
00264 - norm2(sourceCenteredPoint - D1)) > 0 )
00265 switchD1Direction++;
00266 else
00267 switchD1Direction--;
00268 }
00269
00270 zoom /= sourcePointList.size();
00271
00272 const double delta = qvClockWiseAngle((switchD1Direction > 0)?-D1:D1, D2);
00273
00274
00275
00276
00277
00278
00279 QVMatrix result = QVMatrix::identity(3);
00280 result(0,0) = zoom*cos(delta); result(0,1) = zoom*sin(delta); result(0,2) = C2.x() - zoom*cos(delta)*C1.x() - zoom*C1.y()*sin(delta);
00281 result(1,0) = -zoom*sin(delta); result(1,1) = zoom*cos(delta); result(1,2) = C2.y() - zoom*cos(delta)*C1.y() + zoom*C1.x()*sin(delta);
00282
00283 return result;
00284 }
00285
00286 QPointF ApplyHomography(const QVMatrix &H, const QPointF &point)
00287 {
00288 const double h11 = H(0,0), h12 = H(0,1), h13 = H(0,2),
00289 h21 = H(1,0), h22 = H(1,1), h23 = H(1,2),
00290 h31 = H(2,0), h32 = H(2,1), h33 = H(2,2),
00291 x = point.x(), y = point.y(),
00292 homogenizer = h31*x + h32*y + h33;
00293
00294 return QPointF(h11*x + h12*y + h13, h21*x + h22*y + h23)/homogenizer;
00295 }
00296
00297 QList<QPointF> ApplyHomography(const QVMatrix &homography, const QList<QPointF> &sourcePoints)
00298 {
00299 QList<QPointF> result;
00300 foreach(QPointF point, sourcePoints)
00301 result.append(ApplyHomography(homography, point));
00302 return result;
00303 }
00304
00305 double HomographyTestError(const QVMatrix &homography)
00306 {
00307 const QVVector v1 = homography.getCol(0), v2 = homography.getCol(1);
00308 return ABS(v1.norm2() - v2.norm2()) / (v1.norm2() + v2.norm2())
00309 + ABS(v1 * v2) / (v1.norm2() * v2.norm2());
00310 }
00311
00313
00314
00315
00316
00317
00318
00319
00320
00321
00322
00323
00324
00325
00326
00327 void GetDirectIntrinsicCameraMatrixFromHomography(const QVMatrix &H, QVMatrix &K)
00328 {
00329 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);
00330 const double focalNumerator = + (h2*(h2 + h4) - h1*h5 + pow(h5,2))*(pow(h2,2) - h2*h4 + h5*(h1 + h5))*pow(h7,2)
00331 - (pow(h1,2) + pow(h4,2))*(h1*h2 + h4*h5)*h7*h8
00332 + (pow(h1,2) + pow(h4,2))*(pow(h1,2) - pow(h2,2) + pow(h4,2) - pow(h5,2))*pow(h8,2);
00333 const double focalDenominator = + (pow(h2,2) + pow(h5,2))*pow(h7,4)
00334 - (h1*h2 + h4*h5)*pow(h7,3)*h8
00335 - (pow(h2,2) + pow(h5,2))*pow(h7,2)*pow(h8,2)
00336 + (pow(h1,2) + pow(h4,2))*pow(h8,4);
00337 const double finv = sqrt(ABS(focalNumerator / focalDenominator))/2;
00338
00339 K = QVMatrix::identity(3) * finv;
00340 K(2,2) = 1;
00341 }
00342
00343 void CalibrateCameraFromPlanarHomography(const QVMatrix &H, QVMatrix &K, QVMatrix &Rt)
00344 {
00345
00346
00347
00348 GetDirectIntrinsicCameraMatrixFromHomography(H, K);
00349
00350
00351
00352
00353 QVMatrix R12t = pseudoInverse(K)*H;
00354
00355
00356
00357
00358 Rt = QVMatrix(4,4);
00359 R12t = R12t * 2 / (R12t.getCol(0).norm2() + R12t.getCol(1).norm2());
00360
00361
00362 Rt.setCol(0, R12t.getCol(0));
00363 Rt.setCol(1, R12t.getCol(1));
00364 Rt.setCol(2, R12t.getCol(0) ^ R12t.getCol(1));
00365 Rt.setCol(3, R12t.getCol(2));
00366 Rt(3,3) = 1;
00367 }
00368
00369
00370
00371
00372
00373
00374
00375
00376
00377
00378
00379
00380 void GetExtrinsicCameraMatrixFromHomography(const QVMatrix &K, const QVMatrix &H, QVMatrix &Rt)
00381 {
00382
00383 QVMatrix R12_t = pseudoInverse(K)*pseudoInverse(H);
00384
00385
00386
00387
00388 R12_t = R12_t * 2 / (R12_t.getCol(0).norm2() + R12_t.getCol(1).norm2());
00389
00390
00391 Rt = QVMatrix(4,4);
00392
00393 Rt.setCol(0, R12_t.getCol(0));
00394 Rt.setCol(1, R12_t.getCol(1));
00395 Rt.setCol(2, R12_t.getCol(0) ^ R12_t.getCol(1));
00396 Rt.setCol(3, R12_t.getCol(2));
00397 Rt(3,3) = 1;
00398 }
00399
00400 void GetPinholeCameraIntrinsicsFromPlanarHomography(const QVMatrix &H, QVMatrix &K, const int iterations,
00401 const double maxGradientNorm, const double step, const double tol)
00402 {
00403 class KErrorFunction: public QVFunction<QVVector, double>
00404 {
00405 private:
00406 const QVMatrix H;
00407
00408 double evaluate(const QVVector &input) const
00409 {
00410 const QVMatrix Rt = pseudoInverse(KMatrix(input))*H, errorMat = Rt.transpose() * Rt;
00411 return POW2(errorMat(0,0) -1) + POW2(errorMat(1,1) -1) + 2*POW2(errorMat(1,0));
00412 }
00413
00414 public:
00415 KErrorFunction(const QVMatrix &H): QVFunction<QVVector, double>(), H(H) { }
00416
00417 const QVMatrix KMatrix(const QVVector &input) const
00418 {
00419 QVMatrix K = QVMatrix::zeros(3,3);
00420 K(0,0) = input[0];
00421 K(1,1) = input[0];
00422 K(2,2) = input[1];
00423 return K;
00424 }
00425 } errorFunction(H);
00426
00427 QVVector x(2,1);
00428 qvGSLMinimizeFDF(errorFunction, x, VectorBFGS, iterations, maxGradientNorm, step, tol);
00429 K = errorFunction.KMatrix(x);
00430 K = K / K(2,2);
00431 }
00432
00434
00435 QList<QVMatrix> getCanonicalCameraMatricesFromEssentialMatrix(const QVMatrix &E)
00436 {
00437 QVMatrix U, V, S;
00438 singularValueDecomposition(E, U, V, S);
00439
00440 QVMatrix W(3, 3, 0.0), Z(3, 3, 0.0);
00441 W(1,0) = 1; W(0,1) = -1; W(2,2) = 1;
00442 Z(1,0) = -1; Z(0,1) = 1;
00443
00444 const QVMatrix Pa = ( U * W * V.transpose() | U.getCol(2)),
00445 Pb = ( U * W.transpose() * V.transpose() | U.getCol(2));
00446
00447 return QList<QVMatrix>() << Pa << Pb;
00448 }
00449
00450 QVMatrix getEssentialMatrixFromCanonicalCameraMatrix(const QVMatrix &P)
00451 {
00452 return P.getCol(3).crossProductMatrix() * (QVMatrix(P.getCol(0)).transpose() | P.getCol(1) | P.getCol(2));
00453 }
00454
00455 QVMatrix getCameraMatrixFrom2D3DPointCorrespondences(const QList<QPointF> &points2d, const QList<QV3DPointF> &points3d)
00456 {
00457 const int n = points2d.size();
00458 QVMatrix A(3 * n, 12);
00459 for(int i = 0; i < n; i++)
00460 {
00461 const double xp = points2d[i].x(), yp = points2d[i].y(),
00462 x = points3d[i].x(), y = points3d[i].y(), z = points3d[i].z();
00463
00464 const double dataCoefs[3*12] = {
00465 0, 0, 0, 0, -x, -y, -z, -1, +x*yp, +y*yp, +yp*z, +yp,
00466 +x, +y, +z, +1, 0, 0, 0, 0, -x*xp, -xp*y, -xp*z, -xp,
00467 -x*yp, -y*yp, -yp*z, -yp, +x*xp, +xp*y, +xp*z, +xp, 0, 0, 0, 0
00468 };
00469
00470 const QVMatrix coefs(3, 12, dataCoefs);
00471
00472 A.setRow(3*i+0, coefs.getRow(0));
00473 A.setRow(3*i+1, coefs.getRow(1));
00474 A.setRow(3*i+2, coefs.getRow(2));
00475 }
00476
00477 QVVector vectorP;
00478 solveHomogeneousLinear(A, vectorP);
00479
00480 const QVMatrix P = QVMatrix(vectorP.mid(0,4)) & QVMatrix(vectorP.mid(4,4)) & QVMatrix(vectorP.mid(8,4));
00481
00482
00483 const QVMatrix PNormalizer = P.transpose() * P;
00484 return P / sqrt((PNormalizer(0,0) + PNormalizer(1,1) + PNormalizer(2,2)) / 3.0);
00485 }
00486
00487 QV3DPointF triangulate3DPointFrom2DProjectionsAndCanonicalCameraMatrices(const QList<QPointF> &points, const QList<QVMatrix> &Plist)
00488 {
00489 QVMatrix A(2 * points.size(),4);
00490 for(int i = 0; i < points.size(); i++)
00491 {
00492 A.setRow(2*i + 0, Plist[i].getRow(2) * points[i].x() - Plist[i].getRow(0));
00493 A.setRow(2*i + 1, Plist[i].getRow(2) * points[i].y() - Plist[i].getRow(1));
00494
00495
00496
00497
00498
00499 }
00500
00501 QVVector x;
00502
00503 solveHomogeneousLinear(A, x);
00504
00505 if (x[2] / x[3] >= 0)
00506 return QV3DPointF(x[0] / x[3], x[1] / x[3], x[2] / x[3]);
00507 else
00508 return QV3DPointF( - x[0] / x[3], - x[1] / x[3], - x[2] / x[3]);
00509 }
00510