src/qvmath/qvprojective.cpp

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

Generated on Thu Jul 17 17:23:28 2008 for QVision by  doxygen 1.5.3