vpHomography.cpp

/****************************************************************************
 *
 * $Id: vpHomography.cpp 4661 2014-02-10 19:34:58Z 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 transformation.
 *
 * Authors:
 * Muriel Pressigout
 * Fabien Spindler
 *
 *****************************************************************************/


#include <stdio.h>

#include <visp/vpDebug.h>
#include <visp/vpHomography.h>
#include <visp/vpMatrix.h>
#include <visp/vpRobust.h>

// Exception
#include <visp/vpException.h>
#include <visp/vpMatrixException.h>

vpHomography::vpHomography() : data(NULL), aMb(), bP()
{
  data = new double [9];
  setIdentity();
}


vpHomography::vpHomography(const vpHomography &H) : data(NULL), aMb(), bP()
{
  data = new double [9];

  *this = H;
}

vpHomography::vpHomography(const vpHomogeneousMatrix &M, const vpPlane &p) : data(NULL), aMb(), bP()
{
  data = new double [9];
  buildFrom(M, p) ;
}

vpHomography::vpHomography(const vpThetaUVector &tu,
                           const vpTranslationVector &atb,
                           const vpPlane &p) : data(NULL), aMb(), bP()
{
  data = new double [9];
  buildFrom(tu, atb, p) ;
}

vpHomography::vpHomography(const vpRotationMatrix &aRb,
                           const vpTranslationVector &atb,
                           const vpPlane &p) : data(NULL), aMb(), bP()
{
  data = new double [9];
  buildFrom(aRb, atb, p) ;
}

vpHomography::vpHomography(const vpPoseVector &arb, const vpPlane &p) : data(NULL), aMb(), bP()
{
  data = new double [9];
  buildFrom(arb, p) ;
}

vpHomography::~vpHomography()
{
  if (data)
    delete [] data;
  data = NULL;
}

void
vpHomography::buildFrom(const vpHomogeneousMatrix &M,
                        const vpPlane &p)
{
  insert(M) ;
  insert(p) ;
  build() ;
}

void
vpHomography::buildFrom(const vpThetaUVector &tu,
                        const vpTranslationVector &atb,
                        const vpPlane &p)
{
  insert(tu) ;
  insert(atb) ;
  insert(p) ;
  build() ;
}

void
vpHomography::buildFrom(const vpRotationMatrix &aRb,
                        const vpTranslationVector &atb,
                        const vpPlane &p)
{
  insert(aRb) ;
  insert(atb) ;
  insert(p) ;
  build() ;
}

void
vpHomography::buildFrom(const vpPoseVector &arb, const vpPlane &p)
{
  aMb.buildFrom(arb[0],arb[1],arb[2],arb[3],arb[4],arb[5]) ;
  insert(p) ;
  build() ;
}



/*********************************************************************/


void
vpHomography::insert(const vpRotationMatrix &aRb)
{
  aMb.insert(aRb) ;
}

void
vpHomography::insert(const vpHomogeneousMatrix &M)
{
  this->aMb = M ;
}

void
vpHomography::insert(const vpThetaUVector &tu)
{
  vpRotationMatrix aRb(tu) ;
  aMb.insert(aRb) ;
}

void
vpHomography::insert(const vpTranslationVector &atb)
{
  aMb.insert(atb) ;
}

void
vpHomography::insert(const vpPlane &p)
{
  this->bP = p;
}

vpHomography
vpHomography::inverse() const
{
  vpMatrix M = (*this);
  vpMatrix Minv;
  M.pseudoInverse(Minv, 1e-16);

  vpHomography H;

  for(unsigned int i=0; i<3; i++)
    for(unsigned int j=0; j<3; j++)
      H[i][j] = Minv[i][j];

  return H;
}

void
vpHomography::inverse(vpHomography &bHa) const
{
  bHa = inverse() ;
}


void
vpHomography::save(std::ofstream &f) const
{
  if (! f.fail())
  {
    f << *this ;
  }
  else
  {
    throw(vpException(vpException::ioError, "Cannot write the homography to the output stream")) ;
  }
}

vpHomography vpHomography::operator*(const vpHomography &H) const
{
  vpHomography Hp;
  for(unsigned int i = 0; i < 3; i++) {
    for(unsigned int j = 0; j < 3; j++) {
      double s = 0.;
      for(unsigned int k = 0; k < 3; k ++) {
        s += (*this)[i][k] * H[k][j];
      }
      Hp[i][j] = s;
    }
  }
  return Hp;
}

vpColVector
vpHomography::operator*(const vpColVector &b) const
{
  if (b.size() != 3)
    throw(vpException(vpException::dimensionError, "Cannot multiply an homography by a vector of dimension %d", b.size()));

  vpColVector a(3);
  for(unsigned int i=0; i<3; i++) {
    a[i] = 0.;
    for(unsigned int j=0; j<3; j++)
      a[i] += (*this)[i][j] * b[j];
  }

  return a;
}

vpHomography vpHomography::operator*(const double &v) const
{
  vpHomography H;
    
  for (unsigned int i=0; i < 9; i ++) {
    H.data[i] = this->data[i] * v;
  }

  return H;
}

vpHomography vpHomography::operator/(const double &v) const
{
  vpHomography H;
  if (std::fabs(v) <= std::numeric_limits<double>::epsilon())
    throw vpMatrixException(vpMatrixException::divideByZeroError, "Divide by zero in method /=(double v)");

  double vinv = 1/v ;

  for (unsigned int i=0; i < 9; i ++) {
    H.data[i] = this->data[i] * vinv;
  }

  return H;
}

vpHomography & vpHomography::operator/=(double v)
{
  //if (x == 0)
  if (std::fabs(v) <= std::numeric_limits<double>::epsilon())
    throw vpMatrixException(vpMatrixException::divideByZeroError, "Divide by zero in method /=(double v)");

  double vinv = 1/v ;

  for (unsigned int i=0;i<9;i++)
    data[i] *= vinv;

  return *this;
}

vpHomography &
vpHomography::operator=(const vpHomography &H)
{
  for (unsigned int i=0; i< 3; i++)
    for (unsigned int j=0; j< 3; j++)
    (*this)[i][j] = H[i][j];

  return *this;
}
vpHomography &
vpHomography::operator=(const vpMatrix &H)
{
  if (H.getRows() !=3 || H.getCols() != 3)
    throw(vpException(vpException::dimensionError, "The matrix is not an homography"));

  for (unsigned int i=0; i< 3; i++)
    for (unsigned int j=0; j< 3; j++)
    (*this)[i][j] = H[i][j];

  return *this;
}

void
vpHomography::load(std::ifstream &f)
{
  if (! f.fail())
  {
    for (unsigned int i=0 ; i < 3 ; i++)
      for (unsigned int j=0 ; j < 3 ; j++)
      {
        f >> (*this)[i][j] ;
      }
  }
  else
  {
    throw(vpException(vpException::ioError, "Cannot read the homography from the input stream")) ;
  }
}


#ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
void
vpHomography::print()
{
  std::cout <<*this << std::endl ;
}
#endif // #ifdef VISP_BUILD_DEPRECATED_FUNCTIONS

void
vpHomography::build()
{
  vpColVector n(3) ;
  vpColVector atb(3) ;
  vpMatrix aRb(3,3);
  for (unsigned int i=0 ; i < 3 ; i++)
  {
    atb[i] = aMb[i][3];
    for (unsigned int j=0 ; j < 3 ; j++)
      aRb[i][j] = aMb[i][j];
  }

  bP.getNormal(n) ;

  double d = bP.getD() ;
  vpMatrix aHb = aRb - atb*n.t()/d ; // the d used in the equation is such as nX=d is the
             // plane equation. So if the plane is described by
             // Ax+By+Cz+D=0, d=-D

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

void
vpHomography::build(vpHomography &aHb,
                    const vpHomogeneousMatrix &aMb,
                    const vpPlane &bP)
{
  vpColVector n(3) ;
  vpColVector atb(3) ;
  vpMatrix aRb(3,3);
  for (unsigned int i=0 ; i < 3 ; i++)
  {
    atb[i] = aMb[i][3] ;
    for (unsigned int j=0 ; j < 3 ; j++)
      aRb[i][j] = aMb[i][j];
  }

  bP.getNormal(n) ;

  double d = bP.getD() ;
  vpMatrix aHb_ = aRb - atb*n.t()/d ; // the d used in the equation is such as nX=d is the
               // plane equation. So if the plane is described by
               // Ax+By+Cz+D=0, d=-D

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

void
vpHomography::setIdentity()
{
  for (unsigned int i=0 ; i < 3 ; i++)
    for (unsigned int j=0 ; j < 3; j++)
      if (i==j)
        (*this)[i][j] = 1.0 ;
      else
        (*this)[i][j] = 0.0;
}

vpImagePoint vpHomography::project(const vpCameraParameters &cam, const vpHomography &bHa, const vpImagePoint &iPa)
{
  double xa = iPa.get_u();
  double ya = iPa.get_v();
  vpMatrix H = cam.get_K() * bHa * cam.get_K_inverse();
  double z = xa * H[2][0] + ya * H[2][1] + H[2][2];
  double xb = (xa * H[0][0] + ya * H[0][1] + H[0][2]) / z;
  double yb = (xa * H[1][0] + ya * H[1][1] + H[1][2]) / z;

  vpImagePoint iPb(yb, xb);

  return iPb;
}
vpPoint vpHomography::project(const vpHomography &bHa, const vpPoint &Pa)
{
  double xa = Pa.get_x();
  double ya = Pa.get_y();
  double z = xa * bHa[2][0] + ya * bHa[2][1] + bHa[2][2];
  double xb = (xa * bHa[0][0] + ya * bHa[0][1] + bHa[0][2]) / z;
  double yb = (xa * bHa[1][0] + ya * bHa[1][1] + bHa[1][2]) / z;

  vpPoint Pb;
  Pb.set_x(xb);
  Pb.set_y(yb);

  return Pb;
}

void
vpHomography::robust(const std::vector<double> &xb, const std::vector<double> &yb,
                     const std::vector<double> &xa, const std::vector<double> &ya,
                     vpHomography &aHb,
                     std::vector<bool> &inliers,
                     double &residual,
                     double weights_threshold,
                     unsigned int niter,
                     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 robust 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;
    }

    unsigned int nbLinesA = 2;
    vpMatrix A(nbLinesA*n,8);
    vpColVector X(8);
    vpColVector Y(nbLinesA*n);
    vpMatrix W(nbLinesA*n, nbLinesA*n) ; // Weight matrix

    vpColVector w(nbLinesA*n) ;

    // All the weights are set to 1 at the beginning to use a classical least square scheme
    w = 1;
    // Update the square matrix associated to the weights
    for (unsigned int i=0; i < nbLinesA*n; i ++) {
      W[i][i] = w[i];
    }

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

      A[nbLinesA*i+1][0]=0 ;
      A[nbLinesA*i+1][1]=0;
      A[nbLinesA*i+1][2]=0;
      A[nbLinesA*i+1][3]=xbn[i];
      A[nbLinesA*i+1][4]=ybn[i];
      A[nbLinesA*i+1][5]=1;
      A[nbLinesA*i+1][6]=-xbn[i]*yan[i];
      A[nbLinesA*i+1][7]=-ybn[i]*yan[i];

      Y[nbLinesA*i] = xan[i];
      Y[nbLinesA*i+1] = yan[i];
    }

    vpMatrix WA;
    vpMatrix WAp;
    unsigned int iter = 0;
    vpRobust r(nbLinesA*n) ; // M-Estimator

    while (iter < niter) {
      WA = W * A;

      X = WA.pseudoInverse(1e-26)*W*Y;
      vpColVector residu;
      residu = Y - A * X;

      // Compute the weights using the Tukey biweight M-Estimator
      r.setIteration(iter) ;
      r.MEstimator(vpRobust::TUKEY, residu, w) ;

      // Update the weights matrix
      for (unsigned int i=0; i < n*nbLinesA; i ++) {
        W[i][i] = w[i];
      }
      // Build the homography
      for(unsigned int i=0;i<8;i++)
        aHbn.data[i]= X[i];
      aHbn[2][2] = 1;
      {
        vpMatrix aHbnorm = aHbn;
        aHbnorm /= aHbnorm[2][2] ;
      }

      iter ++;
    }
    inliers.resize(n);
    unsigned int nbinliers = 0;
    for(unsigned int i=0; i< n; i++) {
      if (w[i*2] < weights_threshold && w[i*2+1] < weights_threshold)
        inliers[i] = false;
      else {
        inliers[i] = true;
        nbinliers ++;
      }
    }

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

    residual = 0 ;
    vpColVector a(3), b(3), c(3);
    for (unsigned int i=0 ; i < n ; i++) {
      if (inliers[i]) {
        a[0] = xa[i] ; a[1] = ya[i] ; a[2] = 1 ;
        b[0] = xb[i] ; b[1] = yb[i] ; b[2] = 1 ;

        c = aHb*b ; c /= c[2] ;
        residual += (a-c).sumSquare();
      }
    }

    residual = sqrt(residual/nbinliers);
  }
  catch(...)
  {
    throw(vpException(vpException::fatalError, "Cannot estimate an homography")) ;
  }
}

VISP_EXPORT std::ostream &operator <<(std::ostream &s,const vpHomography &H)
{
  std::ios_base::fmtflags original_flags = s.flags();

  s.precision(10) ;
  for (unsigned int i=0;i<H.getRows();i++) {
    for (unsigned int j=0;j<H.getCols();j++){
      s <<  H[i][j] << "  ";
    }
    // We don't add a \n char on the end of the last matrix line
    if (i < H.getRows()-1)
      s << std::endl;
  }

  s.flags(original_flags); // restore s to standard state

  return s;
}