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

Generated on 4 Sep 2015 for Hugintrunk by  doxygen 1.4.7