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