Homography.cpp

Go to the documentation of this file.
00001 /*
00002 * Copyright (C) 2007-2008 Anael Orlinski
00003 *
00004 * This file is part of Panomatic.
00005 *
00006 * Panomatic is free software; you can redistribute it and/or modify
00007 * it under the terms of the GNU General Public License as published by
00008 * the Free Software Foundation; either version 2 of the License, or
00009 * (at your option) any later version.
00010 *
00011 * Panomatic is distributed in the hope that it will be useful,
00012 * but WITHOUT ANY WARRANTY; without even the implied warranty of
00013 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00014 * GNU General Public License for more details.
00015 *
00016 * You should have received a copy of the GNU General Public License
00017 * along with Panomatic; if not, write to the Free Software
00018 * <http://www.gnu.org/licenses/>.
00019 */
00020 
00021 #include <iostream>
00022 #include <float.h>
00023 #include <math.h>
00024 //#include "Tracer.h"
00025 
00026 #include "Homography.h"
00027 
00028 namespace lfeat
00029 {
00030 
00031 const int Homography::kNCols = 8;
00032 
00033 inline int fsign(double x)
00034 {
00035     return (x > 0 ? 1 : (x < 0) ? -1 : 0);
00036 }
00037 
00038 bool Givens(double** C, double* d, double* x, double* r, int N, int n, int want_r);
00039 
00040 Homography::Homography(void) : _nMatches(0)
00041 {
00042     _Amat = NULL;
00043     _Bvec = NULL;
00044     _Rvec = NULL;
00045     _Xvec = NULL;
00046     _v1x = 0;
00047     _v2x = 0;
00048     _v1y = 0;
00049     _v2y = 0;
00050     for (size_t i = 0; i < 3; ++i)
00051     {
00052         for (size_t j = 0; j < 3; ++j)
00053         {
00054             _H[i][j] = 0;
00055         };
00056     };
00057 }
00058 
00059 void Homography::allocMemory(int iNMatches)
00060 {
00061     int aNRows = iNMatches * 2;
00062     _Amat = new double*[aNRows];
00063     for(int aRowIter = 0; aRowIter < aNRows; ++aRowIter)
00064     {
00065         _Amat[aRowIter] = new double[kNCols];
00066     }
00067 
00068     _Bvec = new double[aNRows];
00069     _Rvec = new double[aNRows];
00070     _Xvec = new double[kNCols];
00071 
00072     _nMatches = iNMatches;
00073 }
00074 
00075 void Homography::freeMemory()
00076 {
00077     // free memory
00078     delete[] _Bvec;
00079     delete[] _Rvec;
00080     delete[] _Xvec;
00081     for(int aRowIter = 0; aRowIter < 2 * _nMatches; ++aRowIter)
00082     {
00083         delete[] _Amat[aRowIter];
00084     }
00085     delete[] _Amat;
00086 
00087     // reset number of matches
00088     _nMatches = 0;
00089 
00090 }
00091 
00092 Homography::~Homography()
00093 {
00094     if (_nMatches)
00095     {
00096         freeMemory();
00097     }
00098 }
00099 
00100 void Homography::initMatchesNormalization(PointMatchVector_t& iMatches)
00101 {
00102     // for each set of points (img1, img2), find the vector
00103     // to apply to all points to have coordinates centered
00104     // on the barycenter.
00105 
00106     _v1x = 0;
00107     _v2x = 0;
00108     _v1y = 0;
00109     _v2y = 0;
00110 
00111     //estimate the center of gravity
00112     for (size_t i = 0; i < iMatches.size(); ++i)
00113     {
00114         PointMatchPtr& aMatchIt = iMatches[i];
00115         //aMatchIt->print();
00116 
00117         _v1x += aMatchIt->_img1_x;
00118         _v1y += aMatchIt->_img1_y;
00119         _v2x += aMatchIt->_img2_x;
00120         _v2y += aMatchIt->_img2_y;
00121     }
00122 
00123     _v1x /= (double)iMatches.size();
00124     _v1y /= (double)iMatches.size();
00125     _v2x /= (double)iMatches.size();
00126     _v2y /= (double)iMatches.size();
00127 }
00128 
00129 
00130 std::ostream& operator<< (std::ostream& o, const Homography& H)
00131 {
00132     o << H._H[0][0] << "\t" << H._H[0][1] << "\t" << H._H[0][2] << std::endl
00133         << H._H[1][0] << "\t" << H._H[1][1] << "\t" << H._H[1][2] << std::endl
00134         << H._H[2][0] << "\t" << H._H[2][1] << "\t" << H._H[2][2] << std::endl;
00135 
00136     return o;
00137 }
00138 
00139 void Homography::addMatch(size_t iIndex, PointMatch& iMatch)
00140 {
00141     size_t aRow = iIndex * 2;
00142     double aI1x = iMatch._img1_x - _v1x;
00143     double aI1y = iMatch._img1_y - _v1y;
00144     double aI2x = iMatch._img2_x - _v2x;
00145     double aI2y = iMatch._img2_y - _v2y;
00146 
00147     _Amat[aRow][0] = 0;
00148     _Amat[aRow][1] = 0;
00149     _Amat[aRow][2] = 0;
00150     _Amat[aRow][3] = - aI1x;
00151     _Amat[aRow][4] = - aI1y;
00152     _Amat[aRow][5] = -1;
00153     _Amat[aRow][6] = aI1x * aI2y;
00154     _Amat[aRow][7] = aI1y * aI2y;
00155 
00156     _Bvec[aRow] = aI2y;
00157 
00158     aRow++;
00159 
00160     _Amat[aRow][0] = aI1x;
00161     _Amat[aRow][1] = aI1y;
00162     _Amat[aRow][2] = 1;
00163     _Amat[aRow][3] = 0;
00164     _Amat[aRow][4] = 0;
00165     _Amat[aRow][5] = 0;
00166     _Amat[aRow][6] = - aI1x * aI2x;
00167     _Amat[aRow][7] = - aI1y * aI2x;
00168 
00169     _Bvec[aRow] = - aI2x;
00170 }
00171 
00172 void Homography::transformPoint(double iX, double iY, double& oX, double& oY)
00173 {
00174     double aX = iX - _v1x;
00175     double aY = iY - _v1y;
00176     double aK = double(1. / (_H[2][0] * aX + _H[2][1] * aY + _H[2][2]));
00177 
00178     oX = aK * (_H[0][0] * aX + _H[0][1] * aY + _H[0][2]) + _v2x;
00179     oY = aK * (_H[1][0] * aX + _H[1][1] * aY + _H[1][2]) + _v2y;
00180 }
00181 
00182 bool Homography::estimate(PointMatchVector_t& iMatches)
00183 {
00184     // check the number of matches we need at least 4.
00185     if (iMatches.size() < 4)
00186     {
00187         //TRACE_ERROR("At least 4 matches are needed for homography");
00188         return false;
00189     }
00190 
00191     // create matrices for least-squares solving
00192 
00193     // if there is a different size of matrices set, delete them
00194     if (_nMatches != (int)iMatches.size() && _nMatches != 0)
00195     {
00196         freeMemory();
00197     }
00198 
00199     // if there is no memory allocated, alloc memory
00200     if (_nMatches == 0)
00201     {
00202         allocMemory((int)iMatches.size());
00203     }
00204 
00205     // fill the matrices and vectors with points
00206     for (size_t aFillRow = 0; aFillRow < iMatches.size(); ++aFillRow)
00207     {
00208         addMatch(aFillRow, *(iMatches[aFillRow]));
00209     }
00210 
00211     // solve the system
00212     if (!Givens(_Amat, _Bvec, _Xvec, _Rvec, _nMatches*2, kNCols, 0))
00213     {
00214         //TRACE_ERROR("Failed to solve the linear system");
00215         return false;
00216     }
00217 
00218     // fill the homography matrix with the values.
00219 
00220     _H[0][0] = _Xvec[0];
00221     _H[0][1] = _Xvec[1];
00222     _H[0][2] = _Xvec[2];
00223 
00224     _H[1][0] = _Xvec[3];
00225     _H[1][1] = _Xvec[4];
00226     _H[1][2] = _Xvec[5];
00227 
00228     _H[2][0] = _Xvec[6];
00229     _H[2][1] = _Xvec[7];
00230     _H[2][2] = 1.0;
00231 
00232     return true;
00233 
00234 }
00235 
00236 
00237 
00238 
00239 /*****************************************************************
00240 
00241 Solve least squares Problem C*x+d = r, |r| = min!, by Given rotations
00242 (QR-decomposition). Direct implementation of the algorithm
00243 presented in H.R.Schwarz: Numerische Mathematik, 'equation'
00244 number (7.33)
00245 
00246 If 'd == NULL', d is not accesed: the routine just computes the QR
00247 decomposition of C and exits.
00248 
00249 If 'want_r == 0', r is not rotated back (\hat{r} is returned
00250 instead).
00251 
00252 *****************************************************************/
00253 
00254 bool Givens(double** C, double* d, double* x, double* r, int N, int n, int want_r)
00255 {
00256     int i, j, k;
00257     double w, gamma, sigma, rho, temp;
00258     double epsilon = DBL_EPSILON;       /* FIXME (?) */
00259 
00260     /*
00261     * First, construct QR decomposition of C, by 'rotating away'
00262     * all elements of C below the diagonal. The rotations are
00263     * stored in place as Givens coefficients rho.
00264     * Vector d is also rotated in this same turn, if it exists
00265     */
00266     for (j = 0; j < n; j++)
00267     {
00268         for (i = j + 1; i < N; i++)
00269         {
00270             if (C[i][j])
00271             {
00272                 if (fabs(C[j][j]) < epsilon * fabs(C[i][j]))
00273                 {
00274                     /* find the rotation parameters */
00275                     w = -C[i][j];
00276                     gamma = 0;
00277                     sigma = 1;
00278                     rho = 1;
00279                 }
00280                 else
00281                 {
00282                     w = fsign(C[j][j]) * sqrt(C[j][j] * C[j][j] + C[i][j] * C[i][j]);
00283                     if (w == 0)
00284                     {
00285                         return false;
00286                     }
00287                     gamma = C[j][j] / w;
00288                     sigma = -C[i][j] / w;
00289                     rho = (fabs(sigma) < gamma) ? sigma : fsign(sigma) / gamma;
00290                 }
00291                 C[j][j] = w;
00292                 C[i][j] = rho;  /* store rho in place, for later use */
00293                 for (k = j + 1; k < n; k++)
00294                 {
00295                     /* rotation on index pair (i,j) */
00296                     temp = gamma * C[j][k] - sigma * C[i][k];
00297                     C[i][k] = sigma * C[j][k] + gamma * C[i][k];
00298                     C[j][k] = temp;
00299 
00300                 }
00301                 if (d)          /* if no d vector given, don't use it */
00302                 {
00303                     temp = gamma * d[j] - sigma * d[i];         /* rotate d */
00304                     d[i] = sigma * d[j] + gamma * d[i];
00305                     d[j] = temp;
00306                 }
00307             }
00308         }
00309     }
00310 
00311     if (!d)                     /* stop here if no d was specified */
00312     {
00313         return true;
00314     }
00315 
00316     /* solve R*x+d = 0, by backsubstitution */
00317     for (i = n - 1; i >= 0; i--)
00318     {
00319         double s = d[i];
00320 
00321         r[i] = 0;               /* ... and also set r[i] = 0 for i<n */
00322         for (k = i + 1; k < n; k++)
00323         {
00324             s += C[i][k] * x[k];
00325         }
00326         if (C[i][i] == 0)
00327         {
00328             return false;
00329         }
00330         x[i] = -s / C[i][i];
00331     }
00332 
00333     for (i = n; i < N; i++)
00334     {
00335         r[i] = d[i];    /* set the other r[i] to d[i] */
00336     }
00337 
00338     if (!want_r)                /* if r isn't needed, stop here */
00339     {
00340         return true;
00341     }
00342 
00343     /* rotate back the r vector */
00344     for (j = n - 1; j >= 0; j--)
00345     {
00346         for (i = N - 1; i >= 0; i--)
00347         {
00348             if ((rho = C[i][j]) == 1)           /* reconstruct gamma, sigma from stored rho */
00349             {
00350                 gamma = 0;
00351                 sigma = 1;
00352             }
00353             else if (fabs(rho) < 1)
00354             {
00355                 sigma = rho;
00356                 gamma = sqrt(1 - sigma * sigma);
00357             }
00358             else
00359             {
00360                 gamma = 1 / fabs(rho);
00361                 sigma = fsign(rho) * sqrt(1 - gamma * gamma);
00362             }
00363             temp = gamma * r[j] + sigma * r[i];         /* rotate back indices (i,j) */
00364             r[i] = -sigma * r[j] + gamma * r[i];
00365             r[j] = temp;
00366         }
00367     }
00368     return true;
00369 }
00370 
00371 }

Generated on 31 Aug 2016 for Hugintrunk by  doxygen 1.4.7