00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021 #include <qvmath/qvprojective.h>
00022 #include <qvmath/qvmatrixalgebra.h>
00023 #include <qvcore/qvdefines.h>
00024 #include <qvmath/qvmath.h>
00025 #include <float.h>
00026
00027
00031
00032 QVMatrix ComputeProjectiveHomography(const QList< QPair<QPointF, QPointF> > &matchings)
00033 {
00034 Q_ASSERT(matchings.size() >= 4);
00035
00036 const QList<QPointF> sourcePoints = getFirstPairList<QPointF, QPointF>(matchings),
00037 destPoints = getSecondPairList<QPointF, QPointF>(matchings);
00038
00039 const int num_points = sourcePoints.size();
00040
00041
00042
00043
00044 float minXS=FLT_MAX, maxXS=-FLT_MAX, minYS=FLT_MAX, maxYS=-FLT_MAX,
00045 minXD=FLT_MAX, maxXD=-FLT_MAX, minYD=FLT_MAX, maxYD=-FLT_MAX;
00046 for (int i=0; i<num_points; i++)
00047 {
00048 if(sourcePoints[i].x() < minXS)
00049 minXS = sourcePoints[i].x();
00050 if(sourcePoints[i].x() > maxXS)
00051 maxXS = sourcePoints[i].x();
00052 if(sourcePoints[i].y() < minYS)
00053 minYS = sourcePoints[i].y();
00054 if(sourcePoints[i].y() > maxYS)
00055 maxYS = sourcePoints[i].y();
00056 if(destPoints[i].x() < minXD)
00057 minXD = destPoints[i].x();
00058 if(destPoints[i].x() > maxXD)
00059 maxXD = destPoints[i].x();
00060 if(destPoints[i].y() < minYD)
00061 minYD = destPoints[i].y();
00062 if(destPoints[i].y() > maxYD)
00063 maxYD = destPoints[i].y();
00064 }
00065
00066 if(fabs(minXS-maxXS) < EPSILON)
00067 maxXS += 1.0;
00068 if(fabs(minYS-maxYS) < EPSILON)
00069 maxYS += 1.0;
00070 if(fabs(minXD-maxXD) < EPSILON)
00071 maxXD += 1.0;
00072 if(fabs(minYD-maxYD) < EPSILON)
00073 maxYD += 1.0;
00074
00075
00076
00077 QVMatrix coefsMatrix(3*num_points,9);
00078 for (int n=0;n<num_points;n++)
00079 {
00080 double x = (sourcePoints[n].x()-(maxXS+minXS)/2)/(maxXS-minXS),
00081 y = (sourcePoints[n].y()-(maxYS+minYS)/2)/(maxYS-minYS),
00082 x_p = (destPoints[n].x()-(maxXD+minXD)/2)/(maxXD-minXD),
00083 y_p = (destPoints[n].y()-(maxYD+minYD)/2)/(maxYD-minYD);
00084
00085 coefsMatrix(3*n, 0) = 0; coefsMatrix(3*n, 1) = 0; coefsMatrix(3*n, 2) = 0;
00086 coefsMatrix(3*n, 3) = -x; coefsMatrix(3*n, 4) = -y; coefsMatrix(3*n, 5) = -1;
00087 coefsMatrix(3*n, 6) = x*y_p; coefsMatrix(3*n, 7) = y*y_p; coefsMatrix(3*n, 8) = y_p;
00088
00089 coefsMatrix(3*n+1, 0) = x; coefsMatrix(3*n+1, 1) = y; coefsMatrix(3*n+1, 2) = 1;
00090 coefsMatrix(3*n+1, 3) = 0; coefsMatrix(3*n+1, 4) = 0; coefsMatrix(3*n+1, 5) = 0;
00091 coefsMatrix(3*n+1, 6) = -x*x_p; coefsMatrix(3*n+1, 7) = -y*x_p; coefsMatrix(3*n+1, 8) = -x_p;
00092
00093 coefsMatrix(3*n+2, 0) = -x*y_p; coefsMatrix(3*n+2, 1) = -y*y_p; coefsMatrix(3*n+2, 2) = -y_p;
00094 coefsMatrix(3*n+2, 3) = x*x_p; coefsMatrix(3*n+2, 4) = y*x_p; coefsMatrix(3*n+2, 5) = x_p;
00095 coefsMatrix(3*n+2, 6) = 0; coefsMatrix(3*n+2, 7) = 0; coefsMatrix(3*n+2, 8) = 0;
00096 }
00097
00098
00099 QVVector x(9);
00100 solveHomogeneousLinear(coefsMatrix, x);
00101
00102
00103
00104
00105
00106
00107
00108
00109
00110
00111
00112
00113
00114
00115
00116
00117
00118 QVMatrix homography(3,3);
00119 homography(0,0) = x[0]; homography(0,1) = x[1]; homography(0,2) = x[2];
00120 homography(1,0) = x[3]; homography(1,1) = x[4]; homography(1,2) = x[5];
00121 homography(2,0) = x[6]; homography(2,1) = x[7]; homography(2,2) = x[8];
00122
00124
00125 QVMatrix premult(3,3),postmult(3,3);
00126 premult(0,0) = 1/(maxXS-minXS); premult(0,1) = 0; premult(0,2) = -(maxXS+minXS)/(2*(maxXS-minXS));
00127 premult(1,0) = 0; premult(1,1) = 1/(maxYS-minYS); premult(1,2) = -(maxYS+minYS)/(2*(maxYS-minYS));
00128 premult(2,0) = 0; premult(2,1) = 0; premult(2,2) = 1;
00129
00130 postmult(0,0) = maxXD-minXD; postmult(0,1) = 0; postmult(0,2) = (maxXD+minXD)/2;
00131 postmult(1,0) = 0; postmult(1,1) = maxYD-minYD; postmult(1,2) = (maxYD+minYD)/2;
00132 postmult(2,0) = 0; postmult(2,1) = 0; postmult(2,2) = 1;
00133
00134 homography = (postmult * homography) * premult;
00135
00136
00137
00138
00139
00140
00141 return homography;
00142 }
00143
00144 QVMatrix ComputeHomography(const QList<QPointF> &sourcePoints, const QList<QPointF> &destPoints)
00145 {
00146 Q_ASSERT(sourcePoints.size() == destPoints.size());
00147 Q_ASSERT(sourcePoints.size() >= 4);
00148
00149 std::cerr << "DEPRECATED: ComputeHomography is deprecated. use ComputeProjectiveHomography instead." << std::endl;
00150 return ComputeProjectiveHomography(joinPairList<QPointF, QPointF>(sourcePoints, destPoints));
00151 }
00152
00153 QVMatrix ComputeEuclideanHomography(const QPair<QPointF, QPointF> &firstMatching, const QPair<QPointF, QPointF> &secondMatching)
00154 {
00155 std::cerr << "DEPRECATED: This version of ComputeEuclideanHomography is deprecated. Use the other overloaded version instead, which only takes a list of matchings as input." << std::endl;
00156 const QPointF A1 = firstMatching.first, B1 = secondMatching.first, A2 = firstMatching.second, B2 = secondMatching.second;
00157 const QPointF C1 = (A1 + B1) / 2, C2 = (A2 + B2) / 2;
00158 const double zoom = norm2(A2-B2) / norm2(A1-B1),
00159 delta = qvClockWiseAngle(A1-B1, A2-B2);
00160
00161 QVMatrix result = QVMatrix::identity(3);
00162 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);
00163 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);
00164
00165 return result;
00166 }
00167
00168 void getMeanDirection(const QVMatrix m, QVVector &mean, QVVector &direction)
00169 {
00170 mean = m.meanCol();
00171 QVMatrix centeredM = m;
00172 for (int i = 0; i < centeredM.getCols(); i++)
00173 centeredM.setCol(i, centeredM.getCol(i) - mean);
00174
00175
00176 QVMatrix eigVecs;
00177 QVVector eigVals;
00178 eigenDecomposition(centeredM * centeredM.transpose(), eigVals, eigVecs);
00179
00180 direction = QVVector(eigVecs.getCols());
00181 for (int i = 0; i < eigVecs.getCols(); i++)
00182 direction[i] = eigVecs(0,i);
00183 direction = direction * eigVals[0];
00184 }
00185
00186
00187 QVMatrix ComputeEuclideanHomography(const QList< QPair<QPointF, QPointF> > &matchings)
00188 {
00189
00190 const QList<QPointF> sourcePointList = getFirstPairList<QPointF, QPointF>(matchings),
00191 destPointList = getSecondPairList<QPointF, QPointF>(matchings);
00192
00193 const QVMatrix sources = sourcePointList, destinations = destPointList;
00194
00195 QVVector sourcesMean, destinationsMean, sourcesDirection, destinationsDirection;
00196 getMeanDirection(sources.transpose(), sourcesMean, sourcesDirection);
00197 getMeanDirection(destinations.transpose(), destinationsMean, destinationsDirection);
00198
00199 const QPointF C1 = sourcesMean, C2 = destinationsMean, D1 = sourcesDirection, D2 = destinationsDirection;
00200
00201
00202 double zoom = 0;
00203 int switchD1Direction = 0;
00204 for(int i = 0; i < sourcePointList.size(); i ++)
00205 {
00206 const QPointF sourceCenteredPoint = sourcePointList.at(i) - sourcesMean, destCenteredPoint = destPointList.at(i) - destinationsMean;
00207 zoom += norm2(destCenteredPoint) / norm2(sourceCenteredPoint);
00208
00209 if ( (norm2(destCenteredPoint - D2) - norm2(destCenteredPoint + D2)) * (norm2(sourceCenteredPoint + D1) - norm2(sourceCenteredPoint - D1)) > 0 ||
00210 (norm2(destCenteredPoint - D2) - norm2(destCenteredPoint + D2)) * (norm2(sourceCenteredPoint + D1) - norm2(sourceCenteredPoint - D1)) > 0 )
00211 switchD1Direction++;
00212 else
00213 switchD1Direction--;
00214 }
00215
00216 zoom /= sourcePointList.size();
00217
00218 const double delta = qvClockWiseAngle((switchD1Direction > 0)?-D1:D1, D2);
00219
00220
00221
00222
00223
00224
00225 QVMatrix result = QVMatrix::identity(3);
00226 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);
00227 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);
00228
00229 return result;
00230 }
00231
00232 QPointF ApplyHomography(const QVMatrix &H, const QPointF &point)
00233 {
00234 const double h11 = H(0,0), h12 = H(0,1), h13 = H(0,2),
00235 h21 = H(1,0), h22 = H(1,1), h23 = H(1,2),
00236 h31 = H(2,0), h32 = H(2,1), h33 = H(2,2),
00237 x = point.x(), y = point.y(),
00238 homogenizer = h31*x + h32*y + h33;
00239
00240 return QPointF(h11*x + h12*y + h13, h21*x + h22*y + h23)/homogenizer;
00241 }
00242
00243 QList<QPointF> ApplyHomography(const QVMatrix &homography, const QList<QPointF> &sourcePoints)
00244 {
00245 QList<QPointF> result;
00246 foreach(QPointF point, sourcePoints)
00247 result.append(ApplyHomography(homography, point));
00248 return result;
00249 }
00250
00251 double HomographyTestError(const QVMatrix &homography)
00252 {
00253 const QVVector v1 = homography.getCol(0), v2 = homography.getCol(1);
00254 return ABS(v1.norm2() - v2.norm2()) / (v1.norm2() + v2.norm2())
00255 + ABS(v1 * v2) / (v1.norm2() * v2.norm2());
00256 }
00257
00258
00259
00261
00262
00263 #include <qvmath/qvfunctionminimizer.h>
00264 #ifndef DOXYGEN_IGNORE_THIS
00265 class QVFocalCalibration: public QVFunctionMinimizer
00266 {
00267 private:
00268 QVMatrix H;
00269
00270 public:
00271 QVFocalCalibration(const QVMatrix &Horig): QVFunctionMinimizer(true), H(pseudoInverse(Horig)) {}
00272
00273 virtual double function(const double value)
00274 {
00275
00276
00277
00278
00279
00280
00281
00282 const double *h = H.getWriteData(), f = value;
00283 const double h1 = h[0*3+0], h2 = h[0*3+1],
00284 h4 = h[1*3+0], h5 = h[1*3+1],
00285 h7 = h[2*3+0], h8 = h[2*3+1];
00286
00287 return ( POW2( h1*h2 + h4*h5 + (f)*(f)*h7*h8 ) + POW2( h1*h1 - h2*h2 + h4*h4 - h5*h5 + (f)*(f)*(h7*h7-h8*h8) ) )
00288 / POW2(h1*h1 + h4*h4 + (f)*(f)*h7*h7);
00289 }
00290 };
00291 #endif
00292
00293 void GetIntrinsicCameraMatrixFromHomography(const QVMatrix &H, QVMatrix &K, double focal, const double maxFocal, const int maxIterations, const double maxError)
00294 {
00295 QVFocalCalibration minimizer(H);
00296
00297 minimizer.setStartingValue(focal);
00298 minimizer.setSearchRange(-maxFocal, maxFocal);
00299
00300 const int status = minimizer.iterate(maxIterations, maxError);
00301
00302 focal = minimizer.getMinimum();
00303
00304 std::cout << "Focal = " << focal << std::endl;
00305
00306 Q_ASSERT_X(status == GSL_SUCCESS, "GetIntrinsicCameraMatrixFromHomography", "Minimization method didn't converged: " + status);
00307
00308 K = QVMatrix::identity(3);
00309 K(2,2) = 1/focal;
00310 }
00311
00312
00313
00314
00315
00316
00317
00318
00319
00320
00321
00322
00323 void GetDirectIntrinsicCameraMatrixFromHomography(const QVMatrix &H, QVMatrix &K)
00324 {
00325 const double *h = H.getReadData();
00326 const double h1 = h[0*3+0], h2 = h[0*3+1], h4 = h[1*3+0], h5 = h[1*3+1], h7 = h[2*3+0], h8 = h[2*3+1];
00327 const double focalNumerator = + (h2*(h2 + h4) - h1*h5 + pow(h5,2))*(pow(h2,2) - h2*h4 + h5*(h1 + h5))*pow(h7,2) - (pow(h1,2)
00328 + pow(h4,2))*(h1*h2 + h4*h5)*h7*h8 + (pow(h1,2) + pow(h4,2))*(pow(h1,2) - pow(h2,2) + pow(h4,2) - pow(h5,2))*pow(h8,2);
00329 const double focalDenominator = (pow(h2,2) + pow(h5,2))*pow(h7,4) - (h1*h2 + h4*h5)*pow(h7,3)*h8 - (pow(h2,2) + pow(h5,2))*pow(h7,2)*pow(h8,2) + (pow(h1,2) + pow(h4,2))*pow(h8,4);
00330 const double focal = sqrt(ABS(focalNumerator / focalDenominator))/2;
00331
00332 std::cout << "Focal = " << focal << std::endl;
00333
00334 K = QVMatrix::identity(3);
00335 K(2,2) = 1/focal;
00336 }
00337
00338
00339
00340
00341
00342
00343
00344
00345
00346
00347
00348
00349 void GetExtrinsicCameraMatrixFromHomography(const QVMatrix &K, const QVMatrix &H, QVMatrix &M4x4)
00350 {
00351
00352 QVMatrix M3x3 = pseudoInverse(K)*H;
00353
00354
00355
00356
00357 M3x3 = M3x3 / (0.5 * M3x3.getCol(0).norm2() + 0.5 * M3x3.getCol(1).norm2());
00358
00359
00360 M4x4 = QVMatrix(4,4);
00361
00362 M4x4.setCol(0, M3x3.getCol(0));
00363 M4x4.setCol(1, M3x3.getCol(1));
00364 M4x4.setCol(2, M3x3.getCol(0) ^ M3x3.getCol(1));
00365 M4x4.setCol(3, M3x3.getCol(2));
00366 M4x4(3,3) = 1;
00367 }