PARP Research Group University of Murcia, Spain


src/qvmath/qvprojective.cpp

Go to the documentation of this file.
00001 /*
00002  *      Copyright (C) 2007, 2008, 2009. PARP Research Group.
00003  *      <http://perception.inf.um.es>
00004  *      University of Murcia, Spain.
00005  *
00006  *      This file is part of the QVision library.
00007  *
00008  *      QVision is free software: you can redistribute it and/or modify
00009  *      it under the terms of the GNU Lesser General Public License as
00010  *      published by the Free Software Foundation, version 3 of the License.
00011  *
00012  *      QVision is distributed in the hope that it will be useful,
00013  *      but WITHOUT ANY WARRANTY; without even the implied warranty of
00014  *      MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00015  *      GNU Lesser General Public License for more details.
00016  *
00017  *      You should have received a copy of the GNU Lesser General Public
00018  *      License along with QVision. If not, see <http://www.gnu.org/licenses/>.
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         // 0. Fast normalization (similar to DLT, but based on min and max values
00037         // of X and Y, instead of mean value and normalized mean distance to
00038         // centroid equal to sqrt(2). Hartley-Zisserman 2nd ed. pag 109.
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         // This is to avoid possible div by zero in degenerate conditions:
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         //QPair<QPointF, QPointF> matching;
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         // 0. Fast normalization (similar to DLT, but based on min and max values
00095         // of X and Y, instead of mean value and normalized mean distance to
00096         // centroid equal to sqrt(2). Hartley-Zisserman 2nd ed. pag 109.
00097         QList< QPair<QPointF, QPointF> > homogeneizedPairs;
00098         QVMatrix premult, postmult;
00099 
00100         homogenizePoints(matchings, premult, postmult, homogeneizedPairs);
00101 
00102         // 1. Obtain coeficient matrix for the system:
00103         //      Ax = 0
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         // 2. Solve the homogeneous system.
00127         QVVector x(9);
00128 
00129         solveHomogeneousLinear(coefsMatrix, x);
00130 
00131         // 3. Rearrange elements of vector 'x' in a 3x3 matrix.
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         // 4. De-normalize homography back:
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         // 0. Fast normalization (similar to DLT, but based on min and max values
00150         // of X and Y, instead of mean value and normalized mean distance to
00151         // centroid equal to sqrt(2). Hartley-Zisserman 2nd ed. pag 109.
00152         QList< QPair<QPointF, QPointF> > homogeneizedPairs;
00153         QVMatrix premult, postmult;
00154 
00155         homogenizePoints(matchings, premult, postmult, homogeneizedPairs);
00156 
00157         // 1. Obtain coeficient matrix for the system:
00158         //      Ax = 0
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         // 2. Solve the homogeneous system.
00174         QVVector x(9);
00175         solveHomogeneousLinear(coefsMatrix, x);
00176 
00177         // 3. Rearrange elements of vector 'x' in a 3x3 matrix.
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         // 4. De-normalize homography back:
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         // 1. Compute main direction vector.
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 // Aux functions
00209 /*template <typename T1, typename T2> QList<T1> getFirstPairList(const QList< QPair<T1,T2> > &list)
00210         {
00211         QList<T1> result;
00212         QPair<T1,T2> pair;
00213         foreach(pair, list)
00214                 result.append(pair.first);
00215         return result;
00216         }
00217 
00220 template <typename T1, typename T2> QList<T2> getSecondPairList(const QList< QPair<T1,T2> > &list)
00221         {
00222         QList<T2> result;
00223         QPair<T1,T2> pair;
00224         foreach(pair, list)
00225                 result.append(pair.second);
00226         return result;
00227         }*/
00228 
00229 QVMatrix ComputeEuclideanHomography(const QList< QPair<QPointF, QPointF> > &matchings)
00230         {
00231         // 1. Get source and destination mean point and direction.
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         // 2. Get zoom and angle
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         // 3. Get the euclidean homography matrix, derived from the following code for Mathematica:
00275         //      Rotation[delta_] := { {Cos[delta], Sin[delta], 0}, {-Sin[delta], Cos[delta], 0}, {0, 0, 1} };
00276         //      Translation[x_, y_] := { {1, 0, x}, {0, 1, y}, {0, 0, 1} };
00277         //      Zoom[z_] := { {z, 0, 0}, {0, z, 0}, {0, 0, 1} };
00278         //      result = Translation[C2_x, C2_y].Zoom[zoom].Rotation[delta].Translation[C1_x, C1_y] // MatrixForm
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 // Focal calibration from homography
00314 //
00315 
00316 // To get direct focal distance formula:
00317 //
00318 //      K = {{f, 0, 0}, {0, f, 0}, {0, 0, 1}};
00319 //      Hvar = {{h1, h2, h3}, {h4, h5, h6}, {h7, h8, h9}};
00320 //      ErrorMat[H_, K_] := Transpose[H].Inverse[K].Inverse[K].H;
00321 //      Error[ErrorM_] := ((ErrorM[[1, 1]] - ErrorM[[2, 2]])^2 + ErrorM[[1, 2]]^2) / ErrorM[[1, 1]]^2;
00322 //      ErrorFunction[H_, K_] := Error[ErrorMat[H, K]];
00323 //      P = Dt[ErrorFunction[Hvar, K], f, Constants -> {h1, h2, h3, h4, h5, h6, h7, h8, h9}] ;
00324 //      focalDistance  = Solve[P == 0, f][[2]][[1]] // FullSimplify
00325 //      CForm[focalDistance]
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         // 3.2. Compute intrinsic camera matrix
00346         // K is such that:
00347         // H = K*Rt
00348         GetDirectIntrinsicCameraMatrixFromHomography(H, K);
00349 
00350         // ----------------------------------------------------
00351         // 3.3. Compute extrinsic camera matrix
00352         // 3.3.1. Eliminate K matrix from H
00353         QVMatrix R12t = pseudoInverse(K)*H;
00354 
00355         // 3.3.2. Tricky: homogenize M3x3 such as the lenght of rotation vectors is the unit.
00356         // Divide M3x3 by the mean of the square values for first two elements
00357         // of the diagonal of matrix M3x3^t * M3x3
00358         Rt = QVMatrix(4,4);
00359         R12t = R12t * 2 / (R12t.getCol(0).norm2() + R12t.getCol(1).norm2());
00360 
00361         // 3.3.3. Compose matrix R and -R*C vector in final matrix M
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 // This function decomposes the matrix for an homography, obtaining matrix R and vector T from the decomposition:
00371 //      H = K*M3x3
00372 // where
00373 //      M3x3 = [ R1 | R2 | -R^t*C ]
00374 // obtaining the essencial matrix M of size 3x4:
00375 //              [       |        ]
00376 //      M4x4 =  [  Rot  | -R^t*C ]
00377 //              [ _____ | ______ ]
00378 //              [ 0 0 0 |    1   ]      
00379 //
00380 void GetExtrinsicCameraMatrixFromHomography(const QVMatrix &K, const QVMatrix &H, QVMatrix &Rt)
00381         {
00382         // 1. Eliminate K matrix from H
00383         QVMatrix R12_t = pseudoInverse(K)*pseudoInverse(H);
00384 
00385         // 2. Tricky: homogenize M3x3 such as the lenght of rotation vectors is the unit.
00386         //      2.2 Divide M3x3 by the mean of the square values for first two elements
00387         //      of the diagonal of matrix M3x3^t * M3x3
00388         R12_t = R12_t * 2 / (R12_t.getCol(0).norm2() + R12_t.getCol(1).norm2());
00389 
00390         // 3. Compose matrix R and -R*C vector in final matrix M
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         }



QVision framework. PARP research group, copyright 2007, 2008.