vpHomographyDLT.cpp

/****************************************************************************
 *
 * $Id: vpHomographyDLT.cpp 5052 2014-12-11 14:18:49Z fspindle $
 *
 * This file is part of the ViSP software.
 * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
 * 
 * This software is free software; you can redistribute it and/or
 * modify it under the terms of the GNU General Public License
 * ("GPL") version 2 as published by the Free Software Foundation.
 * See the file LICENSE.txt at the root directory of this source
 * distribution for additional information about the GNU GPL.
 *
 * For using ViSP with software that can not be combined with the GNU
 * GPL, please contact INRIA about acquiring a ViSP Professional 
 * Edition License.
 *
 * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
 * 
 * This software was developed at:
 * INRIA Rennes - Bretagne Atlantique
 * Campus Universitaire de Beaulieu
 * 35042 Rennes Cedex
 * France
 * http://www.irisa.fr/lagadic
 *
 * If you have questions regarding the use of this file, please contact
 * INRIA at visp@inria.fr
 * 
 * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
 * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
 *
 *
 * Description:
 * Homography estimation.
 *
 * Authors:
 * Eric Marchand
 *
 *****************************************************************************/


#include <visp/vpHomography.h>
#include <visp/vpMatrix.h>
#include <visp/vpMatrixException.h>

#include <cmath>    // std::fabs
#include <limits>   // numeric_limits

#ifndef DOXYGEN_SHOULD_SKIP_THIS

void
vpHomography::HartleyNormalization(const std::vector<double> &x, const std::vector<double> &y,
                                   std::vector<double> &xn, std::vector<double> &yn,
                                   double &xg, double &yg, double &coef)
{
  if (x.size() != y.size())
    throw(vpException(vpException::dimensionError,
                      "Hartley normalization require that x and y vector have the same dimension"));

  unsigned int n = (unsigned int) x.size();
  if (xn.size() != n)
    xn.resize(n);
  if (yn.size() != n)
    yn.resize(n);

  xg = 0 ;
  yg = 0 ;

  for (unsigned int i =0 ; i < n ; i++)
  {
    xg += x[i] ;
    yg += y[i] ;
  }
  xg /= n ;
  yg /= n ;

  // Changement d'origine : le centre de gravite doit correspondre
  // a l'origine des coordonnees
  double distance=0;
  for(unsigned int i=0; i<n;i++)
  {
    double xni=x[i]-xg;
    double yni=y[i]-yg;
    xn[i] = xni ;
    yn[i] = yni ;
    distance+=sqrt(vpMath::sqr(xni)+vpMath::sqr(yni));
  }//for translation sur tous les points

  //Changement d'echelle
  distance/=n;
  //calcul du coef de changement d'echelle
  //if(distance ==0)
  if(std::fabs(distance) <= std::numeric_limits<double>::epsilon())
    coef=1;
  else
    coef=sqrt(2.0)/distance;

  for(unsigned int i=0; i<n;i++)
  {
    xn[i] *= coef;
    yn[i] *= coef;
  }
}

void
vpHomography::HartleyNormalization(unsigned int n,
                                   const double *x, const double *y,
                                   double *xn, double *yn,
                                   double &xg, double &yg,
                                   double &coef)
{
  unsigned int i;
  xg = 0 ;
  yg = 0 ;

  for (i =0 ; i < n ; i++)
  {
    xg += x[i] ;
    yg += y[i] ;
  }
  xg /= n ;
  yg /= n ;

  //Changement d'origine : le centre de gravite doit correspondre
  // a l'origine des coordonnees
  double distance=0;
  for(i=0; i<n;i++)
  {
    double xni=x[i]-xg;
    double yni=y[i]-yg;
    xn[i] = xni ;
    yn[i] = yni ;
    distance+=sqrt(vpMath::sqr(xni)+vpMath::sqr(yni));
  }//for translation sur tous les points

  //Changement d'echelle
  distance/=n;
  //calcul du coef de changement d'echelle
  //if(distance ==0)
  if(std::fabs(distance) <= std::numeric_limits<double>::epsilon())
    coef=1;
  else
    coef=sqrt(2.0)/distance;

  for(i=0; i<n;i++)
  {
    xn[i] *= coef;
    yn[i] *= coef;
  }
}

//---------------------------------------------------------------------------------------

void
vpHomography::HartleyDenormalization(vpHomography &aHbn,
                     vpHomography &aHb,
                     double xg1, double yg1, double coef1,
                     double xg2, double yg2, double coef2 )
{

  //calcul des transformations a appliquer sur M_norm pour obtenir M
  //en fonction des deux normalisations effectuees au debut sur
  //les points: aHb = T2^ aHbn T1
  vpMatrix T1(3,3);
  vpMatrix T2(3,3);
  vpMatrix T2T(3,3);

  T1.setIdentity();
  T2.setIdentity();
  T2T.setIdentity();

  T1[0][0]=T1[1][1]=coef1;
  T1[0][2]=-coef1*xg1 ;
  T1[1][2]=-coef1*yg1 ;

  T2[0][0]=T2[1][1]=coef2;
  T2[0][2]=-coef2*xg2 ;
  T2[1][2]=-coef2*yg2 ;

  T2T=T2.pseudoInverse(1e-16) ;

  vpMatrix aHbn_(3,3);
  for(unsigned int i=0; i<3; i++)
    for(unsigned int j=0; j<3; j++)
      aHbn_[i][j] = aHbn[i][j];

  vpMatrix maHb=T2T*aHbn_*T1;

  for (unsigned int i=0 ; i < 3 ; i++)
    for (unsigned int j=0 ; j < 3 ; j++)
      aHb[i][j] = maHb[i][j] ;
}

#endif // #ifndef DOXYGEN_SHOULD_SKIP_THIS

#ifdef VISP_BUILD_DEPRECATED_FUNCTIONS

void
vpHomography::HartleyDLT(unsigned int n,
                         double *xb, double *yb,
                         double *xa, double *ya ,
                         vpHomography &aHb)
{
  try{
    //initialise les donnees initiales
    // code_retour =InitialData(n, p1,p2);

    // normalize points
    double *xbn;
    double *ybn;
    xbn = new double [n];
    ybn = new double [n];

    double xg1, yg1, coef1 ;
    vpHomography::HartleyNormalization(n,
                                       xb,yb,
                                       xbn,ybn,
                                       xg1, yg1,coef1);

    double *xan;
    double *yan;
    xan = new double [n];
    yan = new double [n];

    double xg2, yg2, coef2 ;
    vpHomography::HartleyNormalization(n,
                                       xa,ya,
                                       xan,yan,
                                       xg2, yg2,coef2);

    vpHomography aHbn ;
    //compute the homography using the DLT from normalized data
    vpHomography::DLT(n,xbn,ybn,xan,yan,aHbn);

    //H denormalisee
    vpHomography::HartleyDenormalization(aHbn,aHb,xg1,yg1,coef1,xg2,yg2, coef2);

    delete [] xbn;
    delete [] ybn;
    delete [] xan;
    delete [] yan;

  }
  catch(...)
  {
    vpTRACE(" ") ;
    throw ;
  }
}
#endif //#ifdef VISP_BUILD_DEPRECATED_FUNCTIONS

#ifdef VISP_BUILD_DEPRECATED_FUNCTIONS

void vpHomography::DLT(unsigned int n,
                       double *xb, double *yb,
                       double *xa, double *ya ,
                       vpHomography &aHb)
{

  // 4 point are required
  if(n<4)
  {
    vpTRACE("there must be at least 4 points in the both images\n") ;
    throw ;
  }

  try{
    vpMatrix A(2*n,9);
    vpColVector h(9);
    vpColVector D(9);
    vpMatrix V(9,9);
    
    // We need here to compute the SVD on a (n*2)*9 matrix (where n is
    // the number of points). if n == 4, the matrix has more columns
    // than rows. This kind of matrix is not supported by GSL for
    // SVD. The solution is to add an extra line with zeros
    if (n == 4)
      A.resize(2*n+1,9);

    // build matrix A
    for(unsigned int i=0; i<n;i++)
    {
      A[2*i][0]=0;
      A[2*i][1]=0;
      A[2*i][2]=0;
      A[2*i][3]=-xb[i] ;
      A[2*i][4]=-yb[i] ;
      A[2*i][5]=-1;
      A[2*i][6]=xb[i]*ya[i] ;
      A[2*i][7]=yb[i]*ya[i];
      A[2*i][8]=ya[i];


      A[2*i+1][0]=xb[i] ;
      A[2*i+1][1]=yb[i] ;
      A[2*i+1][2]=1;
      A[2*i+1][3]=0;
      A[2*i+1][4]=0;
      A[2*i+1][5]=0;
      A[2*i+1][6]=-xb[i]*xa[i];
      A[2*i+1][7]=-yb[i]*xa[i];
      A[2*i+1][8]=-xa[i] ;
    }

    // Add an extra line with zero.
    if (n == 4) {
      for (unsigned int i=0; i < 9; i ++) {
        A[2*n][i] = 0;
      }
    }

    // solve Ah = 0
    // SVD  Decomposition A = UDV^T (destructive wrt A)
    A.svd(D,V);

    // on en profite pour effectuer un controle sur le rang de la matrice :
    // pas plus de 2 valeurs singulieres quasi=0
    int rank=0;
    for(unsigned int i = 0; i<9;i++) if(D[i]>1e-7) rank++;
    if(rank <7)
    {
      vpTRACE(" Rank is : %d, should be 8", rank);
      throw(vpMatrixException(vpMatrixException::rankDeficient,
                              "\n\t\t Matrix rank is deficient")) ;
    }
    //h = is the column of V associated with the smallest singular value of A

    // since  we are not sure that the svd implemented sort the
    // singular value... we seek for the smallest
    double smallestSv = 1e30 ;
    unsigned int indexSmallestSv  = 0 ;
    for (unsigned int i=0 ; i < 9 ; i++)
      if ((D[i] < smallestSv) ){ smallestSv = D[i] ;indexSmallestSv = i ; }


    h=V.getCol(indexSmallestSv);

    // build the homography
    for(unsigned int i =0;i<3;i++)
    {
      for(unsigned int j=0;j<3;j++)
        aHb[i][j]=h[3*i+j];
    }

  }
  catch(vpMatrixException me)
  {
    vpTRACE("Matrix Exception ") ;
    throw(me) ;
  }
  catch(vpException me)
  {
    vpERROR_TRACE("caught another error") ;
    std::cout <<std::endl << me << std::endl ;
    throw(me) ;
  }

}
#endif // #ifdef VISP_BUILD_DEPRECATED_FUNCTIONS

void vpHomography::DLT(const std::vector<double> &xb, const std::vector<double> &yb,
                       const std::vector<double> &xa, const std::vector<double> &ya ,
                       vpHomography &aHb,
                       bool normalization)
{
  unsigned int n = (unsigned int) xb.size();
  if (yb.size() != n || xa.size() != n || ya.size() != n)
    throw(vpException(vpException::dimensionError,
                      "Bad dimension for DLT homography estimation"));

  // 4 point are required
  if(n<4)
    throw(vpException(vpException::fatalError, "There must be at least 4 matched points"));

  try{
    std::vector<double> xan, yan, xbn, ybn;

    double xg1=0., yg1=0., coef1=0., xg2=0., yg2=0., coef2=0.;

    vpHomography aHbn;

    if (normalization) {
      vpHomography::HartleyNormalization(xb, yb, xbn, ybn, xg1, yg1, coef1);
      vpHomography::HartleyNormalization(xa, ya, xan, yan, xg2, yg2, coef2);
    }
    else {
      xbn = xb;
      ybn = yb;
      xan = xa;
      yan = ya;
    }

    vpMatrix A(2*n,9);
    vpColVector h(9);
    vpColVector D(9);
    vpMatrix V(9,9);

    // We need here to compute the SVD on a (n*2)*9 matrix (where n is
    // the number of points). if n == 4, the matrix has more columns
    // than rows. This kind of matrix is not supported by GSL for
    // SVD. The solution is to add an extra line with zeros
    if (n == 4)
      A.resize(2*n+1,9);

    // build matrix A
    for(unsigned int i=0; i<n;i++)
    {
      A[2*i][0]=0;
      A[2*i][1]=0;
      A[2*i][2]=0;
      A[2*i][3]=-xbn[i] ;
      A[2*i][4]=-ybn[i] ;
      A[2*i][5]=-1;
      A[2*i][6]=xbn[i]*yan[i] ;
      A[2*i][7]=ybn[i]*yan[i];
      A[2*i][8]=yan[i];


      A[2*i+1][0]=xbn[i] ;
      A[2*i+1][1]=ybn[i] ;
      A[2*i+1][2]=1;
      A[2*i+1][3]=0;
      A[2*i+1][4]=0;
      A[2*i+1][5]=0;
      A[2*i+1][6]=-xbn[i]*xan[i];
      A[2*i+1][7]=-ybn[i]*xan[i];
      A[2*i+1][8]=-xan[i] ;
    }

    // Add an extra line with zero.
    if (n == 4) {
      for (unsigned int  i=0; i < 9; i ++) {
        A[2*n][i] = 0;
      }
    }

    // solve Ah = 0
    // SVD  Decomposition A = UDV^T (destructive wrt A)
    A.svd(D,V);

    // on en profite pour effectuer un controle sur le rang de la matrice :
    // pas plus de 2 valeurs singulieres quasi=0
    int rank=0;
    for(unsigned int i = 0; i<9;i++) if(D[i]>1e-7) rank++;
    if(rank <7)
    {
      vpTRACE(" Rank is : %d, should be 8", rank);
      throw(vpMatrixException(vpMatrixException::rankDeficient,
                              "\n\t\t Matrix rank is deficient")) ;
    }
    //h = is the column of V associated with the smallest singular value of A

    // since  we are not sure that the svd implemented sort the
    // singular value... we seek for the smallest
    double smallestSv = 1e30 ;
    unsigned int indexSmallestSv  = 0 ;
    for (unsigned int i=0 ; i < 9 ; i++)
      if ((D[i] < smallestSv) ){ smallestSv = D[i] ;indexSmallestSv = i ; }

    h=V.getCol(indexSmallestSv);

    // build the homography
    for(unsigned int i =0;i<3;i++)
    {
      for(unsigned int j=0;j<3;j++)
        aHbn[i][j]=h[3*i+j];
    }

    if (normalization) {
      // H after denormalization
      vpHomography::HartleyDenormalization(aHbn, aHb, xg1, yg1, coef1, xg2, yg2, coef2);
    }
    else {
      aHb = aHbn;
    }

  }
  catch(vpMatrixException me)
  {
    vpTRACE("Matrix Exception ") ;
    throw(me) ;
  }
  catch(vpException me)
  {
    vpERROR_TRACE("caught another error") ;
    std::cout <<std::endl << me << std::endl ;
    throw(me) ;
  }
}