vpHomographyDLT.cpp

/****************************************************************************
 *
 * $Id: vpHomographyDLT.cpp 4317 2013-07-17 09:40:17Z fspindle $
 *
 * This file is part of the ViSP software.
 * Copyright (C) 2005 - 2013 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/vpMatrixException.h>

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

void
vpHomography::HartleyNormalization(unsigned int n,
                   double *x, 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 gravité doit correspondre
  // à l'origine des coordonnées
  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'échelle
  distance/=n;
  //calcul du coef de changement d'échelle
  //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 à appliquer sur M_norm pour obtenir M
  //en fonction des deux normalisations effectuées au début 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 maHb=T2T*(vpMatrix)aHbn*T1;

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

}


void
vpHomography::HartleyDLT(unsigned int n,
             double *xb, double *yb,
             double *xa, double *ya ,
             vpHomography &aHb)
{
  try{
    //initialise les données 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 dénormalisée
    vpHomography::HartleyDenormalization(aHbn,aHb,xg1,yg1,coef1,xg2,yg2, coef2);

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

  }
  catch(...)
  {
    vpTRACE(" ") ;
    throw ;
  }
}


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);
    unsigned int i, j;
    
    // 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(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 (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 singulières quasi=0
    int rank=0;
    for(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 (i=0 ; i < 9 ; i++)
      if ((D[i] < smallestSv) ){ smallestSv = D[i] ;indexSmallestSv = i ; }


    h=V.column(indexSmallestSv+1);

    // build the homography
    for(i =0;i<3;i++)
    {
      for(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) ;
  }

}