PARP Research Group University of Murcia, Spain


src/qvmath/qvprojective.cpp

Go to the documentation of this file.
00001 /*
00002  *      Copyright (C) 2007, 2008. 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 
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         // 0. Fast normalization (similar to DLT, but based on min and max values
00041         // of X and Y, instead of mean value and normalized mean distance to
00042         // centroid equal to sqrt(2). Hartley-Zisserman 2nd ed. pag 109.
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         // This is to avoid possible div by zero in degenerate conditions:
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         // 1. Obtain coeficient matrix for the system:
00075         //      Ax = 0
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         // 2. Solve the homogeneous system.
00098         QVVector x(9);
00099         solveHomogeneousLinear(coefsMatrix, x);
00100         //solveHomogeneousLinear2(coefsMatrix, x);
00101 
00102         //x = x / x.norm2();
00103         //x2 = x2 / x2.norm2();
00104         //if (x[0] * x2[0] < 0)
00105         //      x2 = x2 *(-1);
00106 
00107         //if (x2[1] == 0)
00108         /*      {
00109                 std::cout << "-----" << std::endl;
00110                 std::cout << "x1 = " << x << std::endl;
00111                 std::cout << "x2 = " << x2 << std::endl;
00112                 }*/
00113 
00114         //x = x / x[8];
00115 
00116         // 3. Rearrange elements of vector 'x' in a 3x3 matrix.
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         // 4. Renormalize homography back:
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         /*float normalizer=homography(2,2);
00136         for(int i=0;i<3;i++)
00137                 for(int j=0;j<3;j++)
00138                         homography(i,j) /= normalizer;*/
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         // 1. Compute main direction vector.
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         // 1. Get source and destination mean point and direction.
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         // 2. Get zoom and angle
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         // 3. Get the euclidean homography matrix, derived from the following code for Mathematica:
00220         //      Rotation[delta_] := { {Cos[delta], Sin[delta], 0}, {-Sin[delta], Cos[delta], 0}, {0, 0, 1} };
00221         //      Translation[x_, y_] := { {1, 0, x}, {0, 1, y}, {0, 0, 1} };
00222         //      Zoom[z_] := { {z, 0, 0}, {0, z, 0}, {0, 0, 1} };
00223         //      result = Translation[C2_x, C2_y].Zoom[zoom].Rotation[delta].Translation[C1_x, C1_y] // MatrixForm
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 // Focal calibration from homography
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                         // Error function, given an 'f', and a 'H' matrix. In Mathematica:
00275                         //      K = {{f, 0, 0}, {0, f, 0}, {0, 0, 1}};
00276                         //      Hvar = {{h1, h2, h3}, {h4, h5, h6}, {h7, h8, h9}};
00277                         //      ErrorMat[H_, K_] := Transpose[H].Inverse[K].Inverse[K].H;
00278                         //      Error [ErrorM_] := ((ErrorM[[1, 1]] - ErrorM[[2, 2]])^2 + ErrorM[[1, 2]]^ 2) / ErrorM[[1, 1]]^ 2;
00279                         //      ErrorFunction[H_, K_] := Error[ErrorMat[H, K]];
00280                         //      ErrorFunction[Hvar, K] // Simplify
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 // To get direct focal distance formula:
00312 //
00313 //      K = {{f, 0, 0}, {0, f, 0}, {0, 0, 1}};
00314 //      Hvar = {{h1, h2, h3}, {h4, h5, h6}, {h7, h8, h9}};
00315 //      ErrorMat[H_, K_] := Transpose[H].Inverse[K].Inverse[K].H;
00316 //      Error[ErrorM_] := ((ErrorM[[1, 1]] - ErrorM[[2, 2]])^2 + ErrorM[[1, 2]]^2) / ErrorM[[1, 1]]^2;
00317 //      ErrorFunction[H_, K_] := Error[ErrorMat[H, K]];
00318 //      P = Dt[ErrorFunction[Hvar, K], f, Constants -> {h1, h2, h3, h4, h5, h6, h7, h8, h9}] ;
00319 //      focalDistance  = Solve[P == 0, f][[2]][[1]] // FullSimplify
00320 //      CForm[focalDistance]
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 // This function decomposes the matrix for an homography, obtaining matrix R and vector T from the decomposition:
00339 //      H = K*M3x3
00340 // where
00341 //      M3x3 = [ R1 | R2 | -R^t*C ]
00342 // obtaining the essencial matrix M of size 3x4:
00343 //              [       |        ]
00344 //      M4x4 =  [  Rot  | -R^t*C ]
00345 //              [ _____ | ______ ]
00346 //              [ 0 0 0 |    1   ]      
00347 //
00348 void GetExtrinsicCameraMatrixFromHomography(const QVMatrix &K, const QVMatrix &H, QVMatrix &M4x4)
00349         {
00350         // 1. Eliminate K matrix from H
00351         QVMatrix M3x3 = pseudoInverse(K)*H;
00352 
00353         // 2. Tricky: homogenize M3x3 such as the lenght of rotation vectors is the unit.
00354         //      2.2 Divide M3x3 by the mean of the square values for first two elements
00355         //      of the diagonal of matrix M3x3^t * M3x3
00356         M3x3 = M3x3 / (0.5 * M3x3.getCol(0).norm2() + 0.5 * M3x3.getCol(1).norm2());
00357 
00358         // 3. Compose matrix R and -R*C vector in final matrix M
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         }



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