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 void homogenizePoints(const QList< QPair<QPointF, QPointF> > &matchings,
00032 QVMatrix &premult, QVMatrix &postmult,
00033 QList< QPair<QPointF, QPointF> > &homogeneizedPairs
00034 )
00035 {
00036
00037
00038
00039 float minXS=FLT_MAX, maxXS=-FLT_MAX, minYS=FLT_MAX, maxYS=-FLT_MAX,
00040 minXD=FLT_MAX, maxXD=-FLT_MAX, minYD=FLT_MAX, maxYD=-FLT_MAX;
00041
00042 QPair<QPointF, QPointF> matching;
00043 foreach(matching, matchings)
00044 {
00045 minXS = MIN(matching.first.x(), minXS);
00046 maxXS = MAX(matching.first.x(), maxXS);
00047 minYS = MIN(matching.first.y(), minYS);
00048 maxYS = MAX(matching.first.y(), maxYS);
00049
00050 minXD = MIN(matching.second.x(), minXD);
00051 maxXD = MAX(matching.second.x(), maxXD);
00052 minYD = MIN(matching.second.y(), minYD);
00053 maxYD = MAX(matching.second.y(), maxYD);
00054 }
00055
00056
00057 if(fabs(minXS-maxXS) < EPSILON)
00058 maxXS += 1.0;
00059 if(fabs(minYS-maxYS) < EPSILON)
00060 maxYS += 1.0;
00061 if(fabs(minXD-maxXD) < EPSILON)
00062 maxXD += 1.0;
00063 if(fabs(minYD-maxYD) < EPSILON)
00064 maxYD += 1.0;
00065
00066 minXS = 0; maxXS = 320;
00067 minYS = 0; maxYS = 240;
00068
00069
00070 foreach(matching, matchings)
00071 {
00072 const double x = (matching.first.x()-(maxXS+minXS)/2)/(maxXS-minXS),
00073 y = (matching.first.y()-(maxYS+minYS)/2)/(maxYS-minYS),
00074 x_p = (matching.second.x()-(maxXD+minXD)/2)/(maxXD-minXD),
00075 y_p = (matching.second.y()-(maxYD+minYD)/2)/(maxYD-minYD);
00076
00077 homogeneizedPairs.append(QPair<QPointF, QPointF>(QPointF(x,y),QPointF(x_p,y_p)));
00078 }
00079
00080 premult = QVMatrix(3,3), postmult = QVMatrix(3,3);
00081 premult(0,0) = 1/(maxXS-minXS); premult(0,1) = 0; premult(0,2) = -(maxXS+minXS)/(2*(maxXS-minXS));
00082 premult(1,0) = 0; premult(1,1) = 1/(maxYS-minYS); premult(1,2) = -(maxYS+minYS)/(2*(maxYS-minYS));
00083 premult(2,0) = 0; premult(2,1) = 0; premult(2,2) = 1;
00084
00085 postmult(0,0) = maxXD-minXD; postmult(0,1) = 0; postmult(0,2) = (maxXD+minXD)/2;
00086 postmult(1,0) = 0; postmult(1,1) = maxYD-minYD; postmult(1,2) = (maxYD+minYD)/2;
00087 postmult(2,0) = 0; postmult(2,1) = 0; postmult(2,2) = 1;
00088 }
00089
00090 QVMatrix ComputeProjectiveHomography(const QList< QPair<QPointF, QPointF> > &matchings)
00091 {
00092 Q_ASSERT(matchings.size() >= 4);
00093
00094
00095
00096
00097 QList< QPair<QPointF, QPointF> > homogeneizedPairs;
00098 QVMatrix premult, postmult;
00099
00100 homogenizePoints(matchings, premult, postmult, homogeneizedPairs);
00101
00102
00103
00104 QVMatrix coefsMatrix(3*homogeneizedPairs.size(),9);
00105 for (int n = 0; n < homogeneizedPairs.size(); n++)
00106 {
00107 const QPair<QPointF, QPointF> matching = homogeneizedPairs.at(n);
00108 const double x = matching.first.x(),
00109 y = matching.first.y(),
00110 x_p = matching.second.x(),
00111 y_p = matching.second.y();
00112
00113 coefsMatrix(3*n, 0) = 0; coefsMatrix(3*n, 1) = 0; coefsMatrix(3*n, 2) = 0;
00114 coefsMatrix(3*n, 3) = -x; coefsMatrix(3*n, 4) = -y; coefsMatrix(3*n, 5) = -1;
00115 coefsMatrix(3*n, 6) = x*y_p; coefsMatrix(3*n, 7) = y*y_p; coefsMatrix(3*n, 8) = y_p;
00116
00117 coefsMatrix(3*n+1, 0) = x; coefsMatrix(3*n+1, 1) = y; coefsMatrix(3*n+1, 2) = 1;
00118 coefsMatrix(3*n+1, 3) = 0; coefsMatrix(3*n+1, 4) = 0; coefsMatrix(3*n+1, 5) = 0;
00119 coefsMatrix(3*n+1, 6) = -x*x_p; coefsMatrix(3*n+1, 7) = -y*x_p; coefsMatrix(3*n+1, 8) = -x_p;
00120
00121 coefsMatrix(3*n+2, 0) = -x*y_p; coefsMatrix(3*n+2, 1) = -y*y_p; coefsMatrix(3*n+2, 2) = -y_p;
00122 coefsMatrix(3*n+2, 3) = x*x_p; coefsMatrix(3*n+2, 4) = y*x_p; coefsMatrix(3*n+2, 5) = x_p;
00123 coefsMatrix(3*n+2, 6) = 0; coefsMatrix(3*n+2, 7) = 0; coefsMatrix(3*n+2, 8) = 0;
00124 }
00125
00126
00127 QVVector x(9);
00128
00129 solveHomogeneousLinear(coefsMatrix, x);
00130
00131
00132 QVMatrix homography(3,3);
00133 homography(0,0) = x[0]; homography(0,1) = x[1]; homography(0,2) = x[2];
00134 homography(1,0) = x[3]; homography(1,1) = x[4]; homography(1,2) = x[5];
00135 homography(2,0) = x[6]; homography(2,1) = x[7]; homography(2,2) = x[8];
00136
00137
00138 homography = (postmult * homography) * premult;
00139 homography = homography / homography(2,2);
00140
00141 return homography;
00142 }
00143
00144 QVMatrix ComputeFundamentalMatrix(const QList< QPair<QPointF, QPointF> > &matchings)
00145 {
00146 Q_ASSERT(matchings.size() >= 4);
00147
00149
00150
00151
00152 QList< QPair<QPointF, QPointF> > homogeneizedPairs;
00153 QVMatrix premult, postmult;
00154
00155 homogenizePoints(matchings, premult, postmult, homogeneizedPairs);
00156
00157
00158
00159 QVMatrix coefsMatrix(homogeneizedPairs.size(),9);
00160 for (int n=0; n<homogeneizedPairs.size(); n++)
00161 {
00162 const QPair<QPointF, QPointF> matching = homogeneizedPairs.at(n);
00163 const double x = matching.first.x(),
00164 y = matching.first.y(),
00165 x_p = matching.second.x(),
00166 y_p = matching.second.y();
00167
00168 coefsMatrix(n, 0) = x*x_p; coefsMatrix(n, 1) = y*x_p; coefsMatrix(n, 2) = x_p;
00169 coefsMatrix(n, 3) = x*y_p; coefsMatrix(n, 4) = y*y_p; coefsMatrix(n, 5) = y_p;
00170 coefsMatrix(n, 6) = x; coefsMatrix(n, 7) = y; coefsMatrix(n, 8) = 1;
00171 }
00172
00173
00174 QVVector x(9);
00175 solveHomogeneousLinear(coefsMatrix, x);
00176
00177
00178 QVMatrix homography(3,3);
00179 homography(0,0) = x[0]; homography(0,1) = x[1]; homography(0,2) = x[2];
00180 homography(1,0) = x[3]; homography(1,1) = x[4]; homography(1,2) = x[5];
00181 homography(2,0) = x[6]; homography(2,1) = x[7]; homography(2,2) = x[8];
00182
00183
00184 homography = (postmult * homography) * premult;
00185 homography = homography / homography(2,2);
00186
00187 return homography;
00188 }
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< QPair<QPointF, QPointF> > &matchings)
00230 {
00231
00232 QList<QPointF> sourcePointList, destPointList;
00233 QPair<QPointF, QPointF> matching;
00234 foreach(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 }