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

Generated on 5 Dec 2014 for Hugintrunk by  doxygen 1.4.7