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