vpHomographyRansac.cpp

/*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 * Copyright Projet Lagadic / IRISA-INRIA Rennes, 2005
 * www  : http://www.irisa.fr/lagadic
 *+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 *
 * File:      vpHomographyRansac.cpp
 * Project:   ViSP 2.0
 * Author:    Eric Marchand
 * From:      vpHomographyRansac.cpp, ViSP 2
 *
 * Version control
 * ===============
 *
 *  $Id: vpHomographyRansac.cpp 4649 2014-02-07 14:57:11Z fspindle $
 * optimized by Tran to improve speed.
 * ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/

#include <visp/vpHomography.h>
#include <visp/vpColVector.h>
#include <visp/vpRansac.h>

#include <visp/vpImage.h>
#include <visp/vpDisplay.h>
#include <visp/vpMeterPixelConversion.h>

#define vpEps 1e-6

#ifndef DOXYGEN_SHOULD_SKIP_THIS

bool iscolinear(double *x1, double *x2, double *x3);
bool isColinear(vpColVector &p1, vpColVector &p2, vpColVector &p3);

bool
iscolinear(double *x1, double *x2, double *x3)
{
  vpColVector p1(3), p2(3), p3(3);
  p1 << x1 ;
  p2 << x2 ;
  p3 << x3 ;
  //vpColVector v;
  //vpColVector::cross(p2-p1, p3-p1, v);
  //return (v.sumSquare() < vpEps);
  // Assume inhomogeneous coords, or homogeneous coords with equal
  // scale.
  return ((vpColVector::cross(p2-p1, p3-p1).sumSquare()) < vpEps);
}
bool
isColinear(vpColVector &p1, vpColVector &p2, vpColVector &p3)
{
  return ((vpColVector::cross(p2-p1, p3-p1).sumSquare()) < vpEps);
}


bool
vpHomography::degenerateConfiguration(vpColVector &x, unsigned int *ind,
                      double threshold_area)
{

  unsigned int i, j, k;

  for (i=1 ; i < 4 ; i++)
    for (j=0 ; j<i ; j++)
      if (ind[i]==ind[j]) return true ;

  unsigned int n = x.getRows()/4 ;
  double pa[4][3] ;
  double pb[4][3] ;



  for(i = 0 ; i < 4 ; i++)
  {
    pb[i][0] = x[2*ind[i]] ;
    pb[i][1] = x[2*ind[i]+1] ;
    pb[i][2] = 1;

    pa[i][0] = x[2*n+2*ind[i]] ;
    pa[i][1] = x[2*n+2*ind[i]+1] ;
    pa[i][2] = 1;
  }

  i = 0, j = 1, k = 2;

  double area012 = (-pa[j][0]*pa[i][1] + pa[k][0]*pa[i][1] +
            pa[i][0]*pa[j][1] - pa[k][0]*pa[j][1] +
            -pa[i][0]*pa[k][1] + pa[1][j]*pa[k][1]);

  i = 0; j = 1, k = 3;
  double area013 = (-pa[j][0]*pa[i][1] + pa[k][0]*pa[i][1] +
            pa[i][0]*pa[j][1] - pa[k][0]*pa[j][1] +
            -pa[i][0]*pa[k][1] + pa[1][j]*pa[k][1]);

  i = 0; j = 2, k = 3;
  double area023 = (-pa[j][0]*pa[i][1] + pa[k][0]*pa[i][1] +
            pa[i][0]*pa[j][1] - pa[k][0]*pa[j][1] +
            -pa[i][0]*pa[k][1] + pa[1][j]*pa[k][1]);

  i = 1; j = 2, k = 3;
  double area123 = (-pa[j][0]*pa[i][1] + pa[k][0]*pa[i][1] +
            pa[i][0]*pa[j][1] - pa[k][0]*pa[j][1] +
            -pa[i][0]*pa[k][1] + pa[1][j]*pa[k][1]);

  double sum_area = area012 + area013 + area023 + area123;

  return ((sum_area < threshold_area) ||
      (iscolinear(pa[0],pa[1],pa[2]) ||
       iscolinear(pa[0],pa[1],pa[3]) ||
       iscolinear(pa[0],pa[2],pa[3]) ||
       iscolinear(pa[1],pa[2],pa[3]) ||
       iscolinear(pb[0],pb[1],pb[2]) ||
       iscolinear(pb[0],pb[1],pb[3]) ||
       iscolinear(pb[0],pb[2],pb[3]) ||
       iscolinear(pb[1],pb[2],pb[3])));
}
    /*
\brief
Function to determine if a set of 4 pairs of matched  points give rise
to a degeneracy in the calculation of a homography as needed by RANSAC.
This involves testing whether any 3 of the 4 points in each set is
colinear.

point are coded this way
x1b,y1b, x2b, y2b, ... xnb, ynb
x1a,y1a, x2a, y2a, ... xna, yna
leading to 2*2*n
*/
bool
vpHomography::degenerateConfiguration(vpColVector &x, unsigned int *ind)
{
  for (unsigned int i = 1; i < 4 ; i++)
    for (unsigned int j = 0 ;j < i ; j++)
      if (ind[i] == ind[j]) return true ;

  unsigned int n = x.getRows()/4;
  double pa[4][3];
  double pb[4][3];
  unsigned int n2 = 2 * n;
  unsigned int ind2;
  for(unsigned int i = 0; i < 4 ;i++)
    {
      ind2 = 2 * ind[i];
      pb[i][0] = x[ind2];
      pb[i][1] = x[ind2+1];
      pb[i][2] = 1;

      pa[i][0] = x[n2+ind2] ;
      pa[i][1] = x[n2+ind2+1] ;
      pa[i][2] = 1;
    }
  return ( iscolinear(pa[0],pa[1],pa[2]) ||
       iscolinear(pa[0],pa[1],pa[3]) ||
       iscolinear(pa[0],pa[2],pa[3]) ||
       iscolinear(pa[1],pa[2],pa[3]) ||
       iscolinear(pb[0],pb[1],pb[2]) ||
       iscolinear(pb[0],pb[1],pb[3]) ||
       iscolinear(pb[0],pb[2],pb[3]) ||
       iscolinear(pb[1],pb[2],pb[3]));
}
bool
vpHomography::degenerateConfiguration(const std::vector<double> &xb, const std::vector<double> &yb,
                                      const std::vector<double> &xa, const std::vector<double> &ya)
{
  unsigned int n = (unsigned int)xb.size();
  if (n < 4)
    throw(vpException(vpException::fatalError, "There must be at least 4 matched points"));

  std::vector<vpColVector> pa(n), pb(n);
  for (unsigned i=0; i<n;i++) {
    pa[i].resize(3);
    pa[i][0] = xa[i];
    pa[i][1] = ya[i];
    pa[i][2] = 1;
    pb[i].resize(3);
    pb[i][0] = xb[i];
    pb[i][1] = yb[i];
    pb[i][2] = 1;
  }

  for (unsigned int i = 0; i < n-2; i++) {
    for (unsigned int j = i+1; j < n-1; j++) {
      for (unsigned int k = j+1; k < n ; k++)
      {
        if (isColinear(pa[i], pa[j], pa[k])) {
          return true;
        }
        if (isColinear(pb[i], pb[j], pb[k])){
          return true;
        }
      }
    }
  }
  return false;
}
// Fit model to this random selection of data points.
void
vpHomography::computeTransformation(vpColVector &x, unsigned int *ind, vpColVector &M)
{
  unsigned int i ;
  unsigned int n = x.getRows()/4 ;
  std::vector<double> xa(4), xb(4);
  std::vector<double> ya(4), yb(4);
  unsigned int n2 = n * 2;
  unsigned int ind2;
  for(i=0 ; i < 4 ; i++)
    {
      ind2 = 2 * ind[i];
      xb[i] = x[ind2] ;
      yb[i] = x[ind2+1] ;

      xa[i] = x[n2+ind2] ;
      ya[i] = x[n2+ind2+1] ;
    }

  vpHomography aHb ;
  try {
    vpHomography::HLM(xb, yb, xa, ya, true, aHb);
  }
  catch(...)
    {
      aHb.setIdentity();
    }

  M.resize(9);
  for (i=0 ; i <9 ; i++)
    {
      M[i] = aHb.data[i] ;
    }
  aHb /= aHb[2][2] ;
}


// Evaluate distances between points and model.
double
vpHomography::computeResidual(vpColVector &x, vpColVector &M, vpColVector &d)
{
  unsigned int i ;
  unsigned int n = x.getRows()/4 ;
  unsigned int n2 = n  * 2;
  unsigned int i2;
  vpColVector *pa;
  vpColVector *pb;

  pa = new vpColVector [n];
  pb = new vpColVector [n];

  for( i=0 ; i < n ; i++)
    {
      i2 = 2 * i;
      pb[i].resize(3) ;
      pb[i][0] = x[i2] ;
      pb[i][1] = x[i2+1] ;
      pb[i][2] = 1;

      pa[i].resize(3) ;
      pa[i][0] = x[n2+i2] ;
      pa[i][1] = x[n2+i2+1] ;
      pa[i][2] = 1;
    }

  vpMatrix aHb(3,3) ;

  for (i=0 ; i <9 ; i++)
    {
      aHb.data[i] = M[i];
    }

  aHb /= aHb[2][2];

  d.resize(n);

  vpColVector Hpb  ;
  for (i=0 ; i <n ; i++)
    {
      Hpb = aHb*pb[i] ;
      Hpb /= Hpb[2] ;
      d[i] = sqrt((pa[i] - Hpb ).sumSquare()) ;
    }

  delete [] pa;
  delete [] pb;

  return 0 ;
}
#endif //#ifndef DOXYGEN_SHOULD_SKIP_THIS


void
vpHomography::initRansac(unsigned int n,
             double *xb, double *yb,
             double *xa, double *ya,
             vpColVector &x)
{
  x.resize(4*n) ;
  unsigned int n2 = n * 2;
  unsigned int i2;
  for (unsigned int i=0 ; i < n ; i++)
  {
    i2 = 2 * i;
    x[i2] = xb[i] ;
    x[i2+1] = yb[i] ;
    x[n2+i2] = xa[i] ;
    x[n2+i2+1] = ya[i] ;
  }
}

#ifdef VISP_BUILD_DEPRECATED_FUNCTIONS

bool
vpHomography::ransac(unsigned int n,
                     double *xb, double *yb,
                     double *xa, double *ya ,
                     vpHomography &aHb,
                     int consensus,
                     double threshold
                     )
{
  vpColVector x ;
  vpHomography::initRansac(n, xb, yb, xa, ya, x) ;

  vpColVector M ;
  vpColVector inliers(n) ;


  bool ransacable = vpRansac<vpHomography>::ransac(n, x, 4, threshold, M, inliers,  consensus);
  
  if(ransacable)
  {
    for (unsigned int i = 0 ;i < 9 ;i++)
    {
      aHb.data[i] = M[i];
    }
    aHb /= aHb[2][2];
  }
  return ransacable;
}

bool vpHomography::ransac(unsigned int n,
                          double *xb, double *yb,
                          double *xa, double *ya ,
                          vpHomography &bHa,
                          vpColVector &inliers,
                          double /* residual */,
                          int consensus,
                          double threshold,
                          double areaThreshold)
{
  vpColVector x ;
  vpHomography::initRansac(n, xb, yb, xa, ya, x);

  vpColVector M ;

  bool ransacable= vpRansac<vpHomography>::ransac(n, x, 4,
                          threshold, M,
                          inliers, consensus,
                          areaThreshold);

  for (unsigned int i = 0 ;i < 9 ;i++)
  {
    bHa.data[i] = M[i];
  }

  bHa /= bHa[2][2];
  return ransacable;
}
#endif //#ifdef VISP_BUILD_DEPRECATED_FUNCTIONS

bool vpHomography::ransac(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,
                          unsigned int nbInliersConsensus,
                          double threshold,
                          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"));

  vpUniRand random((const long)time(NULL)) ;

  std::vector<unsigned int> best_consensus;
  std::vector<unsigned int> cur_consensus;
  std::vector<unsigned int> cur_outliers;
  std::vector<unsigned int> cur_randoms;

  std::vector<unsigned int> rand_ind;

  unsigned int nbMinRandom = 4 ;
  unsigned int ransacMaxTrials = 1000;
  unsigned int maxDegenerateIter = 1000;

  unsigned int nbTrials = 0;
  unsigned int nbDegenerateIter = 0;
  unsigned int nbInliers = 0;

  bool foundSolution = false;

  std::vector<double> xa_rand(nbMinRandom);
  std::vector<double> ya_rand(nbMinRandom);
  std::vector<double> xb_rand(nbMinRandom);
  std::vector<double> yb_rand(nbMinRandom);

  if (inliers.size() != n)
    inliers.resize(n);

  while (nbTrials < ransacMaxTrials && nbInliers < nbInliersConsensus)
  {
    cur_outliers.clear();
    cur_randoms.clear();

    bool degenerate = true;
    while(degenerate == true){
      std::vector<bool> usedPt(n, false);

      rand_ind.clear();
      for(unsigned int i = 0; i < nbMinRandom; i++)
      {
        // Generate random indicies in the range 0..n
        unsigned int r = (unsigned int)ceil(random()*n) -1;
        while(usedPt[r]) {
          r = (unsigned int)ceil(random()*n) -1;
        }
        usedPt[r] = true;
        rand_ind.push_back(r);

        xa_rand[i] = xa[r];
        ya_rand[i] = ya[r];
        xb_rand[i] = xb[r];
        yb_rand[i] = yb[r];
      }

      try{
        if (! vpHomography::degenerateConfiguration(xb_rand, yb_rand, xa_rand, ya_rand)) {
          vpHomography::DLT(xb_rand, yb_rand, xa_rand, ya_rand, aHb, normalization);
         degenerate = false;
        }
      }
      catch(...){
        degenerate = true;
      }

      nbDegenerateIter ++;

      if (nbDegenerateIter > maxDegenerateIter){
        vpERROR_TRACE("Unable to select a nondegenerate data set");
        throw(vpException(vpException::fatalError, "Unable to select a nondegenerate data set"));
      }
    }

    aHb /= aHb[2][2] ;

    // Computing Residual
    double r = 0;
    vpColVector a(3), b(3), c(3) ;
    for (unsigned int i=0 ; i < nbMinRandom ; i++) {
      a[0] = xa_rand[i] ; a[1] = ya_rand[i] ; a[2] = 1 ;
      b[0] = xb_rand[i] ; b[1] = yb_rand[i] ; b[2] = 1 ;

      c = aHb*b; c /= c[2] ;
      r += (a-c).sumSquare() ;
      //cout << "point " <<i << "  " << (a-c).sumSquare()  <<endl ;;
    }

    // Finding inliers & ouliers
    r = sqrt(r/nbMinRandom);
    //std::cout << "Candidate residual: " << r << std::endl;
    if (r < threshold)
    {
      unsigned int nbInliersCur = 0;
      for (unsigned int i = 0; i < n ; 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] ;
        double error = sqrt((a-c).sumSquare()) ;
        if(error <= threshold){
          nbInliersCur++;
          cur_consensus.push_back(i);
          inliers[i] = true;
        }
        else{
          cur_outliers.push_back(i);
          inliers[i] = false;
        }
      }
      //std::cout << "nb inliers that matches: " << nbInliersCur << std::endl;
      if(nbInliersCur > nbInliers)
      {
        foundSolution = true;
        best_consensus = cur_consensus;
        nbInliers = nbInliersCur;
      }

      cur_consensus.clear();
    }

    nbTrials++;
    if(nbTrials >= ransacMaxTrials){
      vpERROR_TRACE("Ransac reached the maximum number of trials");
      foundSolution = true;
    }
  }

  if(foundSolution){
    if(nbInliers >= nbInliersConsensus)
    {
      std::vector<double> xa_best(best_consensus.size());
      std::vector<double> ya_best(best_consensus.size());
      std::vector<double> xb_best(best_consensus.size());
      std::vector<double> yb_best(best_consensus.size());

      for(unsigned i = 0 ; i < best_consensus.size(); i++)
      {
        xa_best[i] = xa[best_consensus[i]];
        ya_best[i] = ya[best_consensus[i]];
        xb_best[i] = xb[best_consensus[i]];
        yb_best[i] = yb[best_consensus[i]];
      }

      vpHomography::DLT(xb_best, yb_best, xa_best, ya_best, aHb, normalization) ;
      aHb /= aHb[2][2];

      residual = 0 ;
      vpColVector a(3), b(3), c(3);
      for (unsigned int i=0 ; i < best_consensus.size() ; i++) {
        a[0] = xa_best[i] ; a[1] = ya_best[i] ; a[2] = 1 ;
        b[0] = xb_best[i] ; b[1] = yb_best[i] ; b[2] = 1 ;

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

      residual = sqrt(residual/best_consensus.size());
      return true;
    }
    else {
      return false;
    }
  }
  else {
    return false;
  }
}