orsa.cpp

//
// C++ Implementation: stereomatch
//
// Description: eliminate the false matches with epipolar geometry constraint. 
//		See http://www.math-info.univ-paris5.fr/~moisan/epipolar/
//
// Copyright (c) 2007 Lionel Moisan <Lionel.Moisan@parisdescartes.fr>
// Changelog : 2011 Use Eigen SVD <Pierre Moulon>
//
// Copyright: See COPYING file that comes with this distribution
//
//

#include <assert.h>
#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <time.h>
#include "orsa.h"
#include <third_party/Eigen/Cholesky>
#include <third_party/Eigen/Core>
#include <third_party/Eigen/Eigenvalues>
#include <third_party/Eigen/LU>
#include <third_party/Eigen/QR>
#include <third_party/Eigen/SVD>

//
#include <iostream>
//
#ifndef M_PI
#define M_PI 3.14159265358979323846
#endif

/*-------------------- GENERAL PURPOSE ROUTINES --------------------*/

/* routines for vectors and matrices */

float *vector(int nl, int nh)
{
  float *v;
  
  v=(float *)malloc((unsigned) (nh-nl+1)*sizeof(float));
  if (!v) {
      // mwerror(FATAL,1,"allocation failure in vector()");
      fprintf(stderr, "allocation failure in vector()\n");
      exit(EXIT_FAILURE); /* indicate failure.*/
    }
  return v-nl;
}

float **matrix(int nrl, int nrh, int ncl, int nch)
{
  int i;
  float **m;
  
  m=(float **) malloc((unsigned) (nrh-nrl+1)*sizeof(float*));
  if (!m) {
      // mwerror(FATAL,1,"allocation failure 1 in matrix()");
      fprintf(stderr, "allocation failure 1 in matrix()\n");
      exit(EXIT_FAILURE); /* indicate failure.*/
    }
  m -= nrl;
  for(i=nrl;i<=nrh;i++) {
      m[i]=(float *) malloc((unsigned) (nch-ncl+1)*sizeof(float));
      if (!m[i]) {
          // mwerror(FATAL,1,"allocation failure 2 in matrix()");
          fprintf(stderr, "allocation failure 2 in matrix()\n");
          exit(EXIT_FAILURE); /* indicate failure.*/
        }
      m[i] -= ncl;
    }
  return m;
}

void free_vector(float *v, int nl, int nh)
{
  free((char*) (v+nl));

  nh = nh; // to remove the warning "unused parameter ‘nh’"
}

void free_matrix(float **m, int nrl, int nrh, int ncl, int nch)
{
  int i;
  
  for(i=nrh;i>=nrl;i--) free((char*) (m[i]+ncl));
  free((char*) (m+nrl));

  nch = nch; // to remove the warning "unused parameter ‘nh’"
}

/* Compute the real roots of a third order polynomial */
/* returns 1 or 3, the number of roots found */

int FindCubicRoots(float coeff[4], float x[3])
{
  float a1 = coeff[2] / coeff[3];
  float a2 = coeff[1] / coeff[3];
  float a3 = coeff[0] / coeff[3];
  
  double Q = (a1 * a1 - 3 * a2) / 9;
  double R = (2 * a1 * a1 * a1 - 9 * a1 * a2 + 27 * a3) / 54;
  double Qcubed = Q * Q * Q;
  double d = Qcubed - R * R;
  
  /* Three real roots */
  if (d >= 0) {
      double theta = acos(R / sqrt(Qcubed));
      double sqrtQ = sqrt(Q);
      x[0] = -2 * sqrtQ * cos( theta             / 3) - a1 / 3;
      x[1] = -2 * sqrtQ * cos((theta + 2 * M_PI) / 3) - a1 / 3;
      x[2] = -2 * sqrtQ * cos((theta + 4 * M_PI) / 3) - a1 / 3;
      return (3);
    }
  
  /* One real root */
  else {
      double e = pow(sqrt(-d) + fabs(R), 1. / 3.);
      if (R > 0)
        e = -e;
      x[0] = (e + Q / e) - a1 / 3.;
      return (1);
    }
}


/* logarithm (base 10) of binomial coefficient */
float logcombi(int k, int n)
{
  double r;
  int i;

  if (k>=n || k<=0) return(0.);
  if (n-k<k) k=n-k;
  r = 0.;
  for (i=1;i<=k;i++)
    r += log10((double)(n-i+1))-log10((double)i);

  return((float)r);
}

/* tabulate logcombi(.,n) */
float *makelogcombi_n(int n)
{
  float *l;
  int k;

  l = (float *)malloc((n+1)*sizeof(float));
  for (k=0;k<=n;k++) l[k]=logcombi(k,n);

  return(l);
}

/* tabulate logcombi(k,.) */
float *makelogcombi_k(int k, int nmax)
{
  float *l;
  int n;

  l = (float *)malloc((nmax+1)*sizeof(float));
  for (n=0;n<=nmax;n++) l[n]=logcombi(k,n);

  return(l);
}


/* get a (sorted) random 7-uple of 0..n-1 */
void random_p7(int *k, int n)
{
  int i,j,j0,r;

  for (i=0;i<7;i++) {
      r = (rand()>>3)%(n-i);
      for (j=0;j<i && r>=k[j];j++) r++;
      j0 = j;
      for (j=i;j>j0;j--) k[j]=k[j-1];
      k[j0]=r;
    }
}


///Mishkin
void multiply3x3 (double* L, double* R, double* res)
{
    res[0] = L[0]*R[0]+L[1]*R[3]+L[2]*R[6];
    res[1] = L[0]*R[1]+L[1]*R[4]+L[2]*R[7];
    res[2] = L[0]*R[2]+L[1]*R[5]+L[2]*R[8];

    res[3] = L[3]*R[0]+L[4]*R[3]+L[5]*R[6];
    res[4] = L[3]*R[1]+L[4]*R[4]+L[5]*R[7];
    res[5] = L[3]*R[2]+L[4]*R[5]+L[5]*R[8];

    res[6] = L[6]*R[0]+L[7]*R[3]+L[8]*R[6];
    res[7] = L[6]*R[1]+L[7]*R[4]+L[8]*R[7];
    res[8] = L[6]*R[2]+L[7]*R[5]+L[8]*R[8];
}

///
/*-------------------- END OF GENERAL PURPOSE ROUTINES --------------------*/


/* float comparison for qsort() */
//According to http://www.cplusplus.com/reference/clibrary/cstdlib/qsort/, 
//we should have: void qsort ( void * base, size_t num, size_t size, int ( * comparator ) ( const void *, const void * ) ); that means, for "qsort", the "comparator" has two constant void* type input parameters
// static int compf(void *i, void *j)
int compf(const void *i, const void *j)
{ 
  float a,b;

  a = *((float *)i);
  b = *((float *)j);
  return(a<b?-1:(a>b?1:0));
}



/* find the increasing sequence of squared distances to epipolar lines */
/* e[n*2] = distances, e[n*2+1] = indexes (to cast into an int) */

//void matcherrorn(float **F, Flist p1, Flist p2, float *e)
void matcherrorn(float **F, const std::vector<float>& p1, const std::vector<float>& p2, float *e)
{
//  int i;
//  double x,y,a,b,c,d; // Guoshen Yu, double precision is needed. When the two images are identical, the error under float precision is 0 => log(error)=-inf.

//  int pt_size = (p1.size())/2;

//  for (i = 0; i < pt_size; i++) {
//      x = (double) p1[i*2];
//      y = (double) p1[i*2+1];
//      a = (double) F[1][1]*x+(double) F[1][2]*y+(double) F[1][3]; //rxc
//      b = (double) F[2][1]*x+(double) F[2][2]*y+(double) F[2][3]; //ryc
//      c = (double) F[3][1]*x+(double) F[3][2]*y+(double) F[3][3]; //rwc
//      d = (a*(double) p2[i*2]+b*(double) p2[i*2+1]+c); //r
//      e[i*2] = (d*d)/(a*a+b*b);


      int i;
      double x1,y1,x2,y2,rxc,ryc,rwc,rx,ry,r,a,b; // Guoshen Yu, double precision is needed. When the two images are identical, the error under float precision is 0 => log(error)=-inf.

      int pt_size = (p1.size())/2;

      for (i = 0; i < pt_size; i++) {
          x1 = (double) p1[i*2];
          y1 = (double) p1[i*2+1];
          x2 = (double) p2[i*2];
          y2 = (double) p2[i*2+1];


          rxc = (double) F[1][1]*x2+(double) F[2][1]*y2+(double) F[3][1];
          ryc = (double) F[1][2]*x2+(double) F[2][2]*y2+(double) F[3][2];
          rwc = (double) F[1][3]*x2+(double) F[2][3]*y2+(double) F[3][3];

          r = (rxc*x1+ryc*y1+rwc);

          rx = (double) F[1][1]*x1+(double) F[1][2]*y1+(double) F[1][3];
          ry = (double) F[2][1]*x1+(double) F[2][2]*y1+(double) F[2][3];
          a = rxc*rxc + ryc*ryc;
          b = rx*rx + ry*ry;

          e[i*2] = r*r*(a+b)/(a*b); //Symmetric
     //   e[i*2] = r*r / (a+b); //Sampson error

          e[i*2+1] = (float)i;
    }
  qsort(e, pt_size, 2*sizeof(float), compf);
}


/*---------- compute the epipolar geometry associated to 7 pairs ----------*/
/*                                                                         */
/*  INPUT: the points are (m1[k[i]*2],m1[k[i]*2+1]), m2... 0<i<7           */
/*                                                                         */
/*  OUTPUT:                                                                */
/*             return the number of roots found, stored in z[]             */
/*   the epipolar matrices are F1[i][j]+z[k]*F2[i][j], 1<=i,j<=3, 0<=k<m   */

// int epipolar(float *m1, float *m2, int *k, float *z, float **F1, float **F2)
int epipolar(std::vector<float>& m1, std::vector<float>& m2, int *k, float *z, float **F1, float **F2)
{
  float a[4];
  int i,j,i2,i3;

  typedef Eigen::MatrixXf Mat;
  Mat c(7,9);
  /* build 9xn matrix from point matches */
  for (i=0;i<7;i++) {
      c(i,0) = m1[k[i]*2  ]*m2[k[i]*2  ];
      c(i,1) = m1[k[i]*2+1]*m2[k[i]*2  ];
      c(i,2) =                      m2[k[i]*2  ];
      c(i,3) = m1[k[i]*2  ]*m2[k[i]*2+1];
      c(i,4) = m1[k[i]*2+1]*m2[k[i]*2+1];
      c(i,5) =                      m2[k[i]*2+1];
      c(i,6) = m1[k[i]*2  ];
      c(i,7) = m1[k[i]*2+1];
      c(i,8) = 1.;
    }
  
  // SVD
  Eigen::JacobiSVD<Mat> svd(c, Eigen::ComputeFullV);
  // look for the two smallest eigenvalue of c'c
  typedef Eigen::Matrix<float, 9, 1> Vec9;
  Vec9 F1Vec = svd.matrixV().col(c.cols()-1);
  Vec9 F2Vec = svd.matrixV().col(c.cols()-2);
  
  /* build basis of solutions */
  int cpt = 0;
  for (i=1;i<=3;i++)
    for (j=1;j<=3;j++)
      {
        F1[i][j] = F1Vec(cpt);
        F2[i][j] = F2Vec(cpt);
        cpt++;
      }
  
  /* build cubic polynomial P(x)=det(F1+xF2) */
  a[0] = a[1] = a[2] = a[3] = 0.;
  for (i=1;i<=3;i++) {
      i2 = i%3+1;
      i3 = i2%3+1;
      a[0] += F1[i][1]*F1[i2][2]*F1[i3][3];
      a[1] +=
          F2[i][1]*F1[i2][2]*F1[i3][3]+
          F1[i][1]*F2[i2][2]*F1[i3][3]+
          F1[i][1]*F1[i2][2]*F2[i3][3];
      a[2] +=
          F1[i][1]*F2[i2][2]*F2[i3][3]+
          F2[i][1]*F1[i2][2]*F2[i3][3]+
          F2[i][1]*F2[i2][2]*F1[i3][3];
      a[3] += F2[i][1]*F2[i2][2]*F2[i3][3];
    }
  for (i=1;i<=3;i++) {
      i2 = (i+1)%3+1;
      i3 = (i2+1)%3+1;
      a[0] -= F1[i][1]*F1[i2][2]*F1[i3][3];
      a[1] -=
          F2[i][1]*F1[i2][2]*F1[i3][3]+
          F1[i][1]*F2[i2][2]*F1[i3][3]+
          F1[i][1]*F1[i2][2]*F2[i3][3];
      a[2] -=
          F1[i][1]*F2[i2][2]*F2[i3][3]+
          F2[i][1]*F1[i2][2]*F2[i3][3]+
          F2[i][1]*F2[i2][2]*F1[i3][3];
      a[3] -= F2[i][1]*F2[i2][2]*F2[i3][3];
    }
  
  return(FindCubicRoots(a,z));
}

void divide_match(const std::vector<Match>& matches, std::vector<float>& p1, std::vector<float>& p2)
{
  float x1, y1, x2, y2;

  p1.clear();
  p2.clear();
  p1.reserve(2 * matches.size());
  p2.reserve(2 * matches.size());
  std::vector<Match>::const_iterator it=matches.begin();
  for(; it != matches.end(); ++it) {
      x1 = (*it).x1; y1 = (*it).y1;
      x2 = (*it).x2; y2 = (*it).y2;
      p1.push_back(x1); p1.push_back(y1);
      p2.push_back(x2); p2.push_back(y2);
    }
}


// float stereomatch(int img_x, int img_y, int size_pt, float* p1, float* p2, float** f, float* index, int* t, int* verb, int* n_flag, int* mode, int* stop)
// float stereomatch(const wxImage& u1, std::vector<float>& p1, std::vector<float>& p2, std::vector<SmallVector<float,3> >& f, std::vector<float>& index, int* t, int* verb, int* n_flag, int* mode, int* stop)
//int main(int argc, char** argv)
float orsa(int width, int height, std::vector<Match>& match, std::vector<float>& index, int t_value, int verb_value, int n_flag_value, int mode_value, int stop_value, double *Fout)
{
  //   int width = 0, height = 0;
  //  int t_value, verb_value, n_flag_value, mode_value, stop_value;
  int *t, *verb, *n_flag, *mode, *stop;

  t = (int*)malloc(sizeof(int)); // maximum number of ransac trials
  verb = (int*)malloc(sizeof(int)); //verbose
  n_flag = (int*)malloc(sizeof(int)); // in order NOT to reinitialize the random seed
  mode = (int*)malloc(sizeof(int)); // mode: 0=deterministic 1=ransac 2=optimized ransac (ORSA) 3=automatic
  stop = (int*)malloc(sizeof(int)); // stop as soon as the first meaningful correspondence is found

  if(width <=0 || height <= 0) {
      std::cerr << "Wrong dimensions of image" << std::endl;
      return 1;
    }

  std::vector<float> p1(2*match.size()), p2(2*match.size()), p1_backup(2*match.size()), p2_backup(2*match.size());

  divide_match(match, p1, p2);
  p1_backup = p1;
  p2_backup = p2;

  libNumerics::matrix<libNumerics::flnum> f(3, 3);
  f = 0;
  index = std::vector<float>(match.size());
  // Guoshen Yu, 2010.09.23
  // index.clear();

  if(t_value <= 0) {
      std::cerr << "t should be greater than 0" << std::endl;
      return 1;
    }
  *t = t_value;

  if(verb_value == 0) {
      free(verb);
      verb = NULL;
    }
  else
    *verb = verb_value;
  if(verb_value != 1 && verb_value != 0) {
      std::cerr << "verb can only be 0 or 1" << std::endl;
      return 1;
    }

  if(n_flag_value == 0) {
      free(n_flag);
      n_flag = NULL;
    }
  else
    *n_flag = n_flag_value;
  if(n_flag_value != 1 && n_flag_value != 0) {
      std::cerr << "n_flag can only be 0 or 1" << std::endl;
      return 1;
    }

  if(mode_value != 0 && mode_value != 1 && mode_value != 2 && mode_value != 3) {
      std::cerr << "mode can only be 0 or 1 or 2 or 3" << std::endl;
      return 1;
    }
  *mode = mode_value;

  if(stop_value == 0) {
      free(stop);
      stop = NULL;
    }
  else
    *stop = stop_value;
  if(stop_value != 1 && stop_value != 0) {
      std::cerr << "stop can only be 0 or 1" << std::endl;
      return 1;
    }


  int i,j,i0,k[8],idk[8],*id,m,n,l,minicur=0,miniall=0,delete_index,nid;
  int niter,maxniter,better,cont,optimization;
  float **F1,**F2,**F,nx,ny,z[3],minepscur,minepsall,nfa;
  float norm,minlogalphacur,minlogalphaall,logalpha,logalpha0;
  float *e,*logcn,*logc7,loge0;

  /* initialize random seed if necessary */
  // if (!n_flag) srand48( (long int) time (NULL) + (long int) getpid() );
  // if (!n_flag) srand( (long int) time (NULL) + (long int) getpid() );

  // Guoshen Yu, 2010.09.21: remove getpid which does not exist under Windows
  if (!n_flag) srand( (long int) time (NULL) );
  
  /* check sizes */
  if (p1.size() != p2.size() || p1.size() < 14) {
      fprintf(stderr, "Inconsistent sizes.\n");
      exit(EXIT_FAILURE); /* indicate failure.*/
    }
  n = p1.size()/2;
  
  /* tabulate logcombi */
  loge0 = (float)log10(3.*(double)(n-7));
  logcn = makelogcombi_n(n);
  logc7 = makelogcombi_k(7,n);
  
  /* choose mode */
  if (*mode==3) {
      if (logcn[7]<=(float)log10((double)(*t)))
        *mode=0;
      else *mode=2;
    }
  if (verb)
    switch(*mode) {
      case 0:
        //	i = (int)(0.5+pow(10.,logc7[n]));
        // Guoshen Yu, 2010.09.22, Windows version
        i = (int)(0.5+pow(10., (double)(logc7[n])));
        printf("I will use deterministic mode (systematic search).\n");
        printf("I have to test %d bases\n",i);
        break;
      case 1:
        printf("I will use pure stochastic mode with no optimization.\n");
        break;
      case 2:
        printf("I will use optimized stochastic mode (ORSA).\n");
      }

  /* normalize coordinates */ //!!!!!!!!!!!!!!!!!!!!!!
  nx = (float)width;
  ny = (float)height;
  norm = 1./(float)sqrt((double)(nx*ny));
  logalpha0 = (float)(log10(2.)+0.5*log10((double)((nx*nx+ny*ny)*norm*norm)));
  for (i=0;i<n;i++) {
      p1[i*2  ] =  (p1[i*2  ]-0.5*nx)*norm;
      p1[i*2+1] =  (p1[i*2+1]-0.5*ny)*norm;
      p2[i*2  ] =  (p2[i*2  ]-0.5*nx)*norm;
      p2[i*2+1] =  (p2[i*2+1]-0.5*ny)*norm;
    }

  /* allocate and initialize memory */
  F  = matrix(1,3,1,3);
  F1  = matrix(1,3,1,3);
  F2  = matrix(1,3,1,3);


  //delete_index = (index?0:1);
  delete_index = 0;

  e = (float *)malloc(2*n*sizeof(float));
  id = (int *)malloc(n*sizeof(int));
  for (i=0;i<n;i++) id[i]=i;
  nid = n;

  maxniter = (*mode==0?*t:*t-(*t)/10);
  minlogalphaall = minepsall = 10000.;
  niter = optimization = 0;
  i0=0; k[0]=-1; k[7]=n;

  /********** MAIN LOOP **********/
  do {

      niter++;

      /* build next list of points */
      if (*mode) random_p7(k,nid);
      else {
          k[i0]++;
          for (i=i0+1;i<=6;i++) k[i]=k[i-1]+1;
        }
      for (i=0;i<7;i++) idk[i]=id[k[i]];

      /* find epipolar transform */
      m = epipolar(p1,p2,idk,z,F1,F2);
      
      /* loop on roots */
      for (;m--;) {

          for (i=1;i<=3;i++)
            for (j=1;j<=3;j++)
              F[i][j] = F1[i][j]+z[m]*F2[i][j];

          /* sort errors */
          matcherrorn(F,p1,p2,e);

          ///
//          for (i=1;i<=3;i++)
//            {for (j=1;j<=3;j++)
//                std::cout << F[i][j] << " ";
//              std::cout << std::endl;
//            }
//           std::cout << std::endl;
          ///
          /* find most meaningful subset */
          minepscur = minlogalphacur = 10000.;
          for (i=7;i<n;i++) {
              logalpha = logalpha0+0.5*(float)log10((double)e[i*2]);
              nfa = loge0+logalpha*(float)(i-6)+logcn[i+1]+logc7[i+1];
              if (nfa<minepscur) {
                  minepscur = nfa;
                  minicur = i;
                  minlogalphacur = logalpha;
                }
            }
          if (minepscur<minepsall) {
              /* store best result so far */
              better = 1;
              minepsall = minepscur;
              minlogalphaall = minlogalphacur;
              miniall = minicur;
              // if (f)
              for (l=1;l<=3;l++)
                for (j=1;j<=3;j++)
                  f(l-1, j-1) = F[l][j];

              // Guoshen Yu, 2010.09.22
              //  for (i=0;i<=minicur;i++)
              for (i=0;i<minicur;i++)
                {
                  index[i] = e[i*2+1];
                }
            } else better=0;


          if (*mode==2 && ((better && minepsall<0.) ||
                           (niter==maxniter && !optimization))) {
              if (!optimization) maxniter = niter + (*t)/10;
              optimization = 1;
              /* final optimization */
              if (verb) {
                  printf("   nfa=%f size=%d (niter=%d)\n",minepsall,miniall+1,niter);
                  printf("optimization...\n");
                }
              nid = miniall+1;

              // Guoshen Yu, 2010.09.22
              // for (j=0;j<=miniall;j++)
              for (j=0;j<miniall;j++)
                id[j] = (int)(index[j]);
            }
        }

      /* prepare next list of points */
      if (*mode==0)
        for(i0=6;i0>=0 && k[i0]==k[i0+1]-1;i0--){};

      if (stop && minepsall<0.) cont=0;
      else if (*mode==0) cont=(i0>=0?1:0);
      else cont=(niter<maxniter?1:0);

    } while (cont);


  //erase "index", only get the index of the meaningful matchings
  index.erase(index.begin()+miniall+1, index.end());
  if (verb)
    printf("best matching found:  %d points  log(alpha)=%f  (%d iterations)\n",
           miniall+1,minlogalphaall,niter);


  for (int a=0; a < 3; a++) //Mishkin; Save fundamental matrix
    for (int b=0; b < 3; b++)
      Fout[a*3  + b] = f(a, b);

  //F denormalization

//            for (i=1;i<=3;i++)
//              {for (j=1;j<=3;j++)
//                  std::cout << Fout[(i-1)*3  + (j-1)] << " ";
//                std::cout << std::endl;
//              }
//             std::cout << std::endl;

  double T[3*3], Tt[3*3];

  T[0] = norm; T[1] = 0;    T[2] = -0.5*nx*norm;
  T[3] = 0;    T[4] = norm; T[5] = -0.5*ny*norm;
  T[6] = 0;    T[7] = 0;    T[8] = 1.0;

  Tt[0] = T[0];    Tt[1] = T[3];    Tt[2] = T[6];
  Tt[3] = T[1];    Tt[4] = T[4];    Tt[5] = T[7];
  Tt[6] = T[2];    Tt[7] = T[5];    Tt[8] = T[8];

  double Ftemp[9];
 multiply3x3(Tt,Fout,Ftemp);
 multiply3x3(Ftemp,T,Fout);

// for (i=1;i<=3;i++)
//   {for (j=1;j<=3;j++)
//       std::cout << Fout[(i-1)*3  + (j-1)] << " ";
//     std::cout << std::endl;
//   }
//  std::cout << std::endl;

 //
  /* free memory */
  free(id);
  free(e);
  // if (delete_index) mw_delete_flist(index);
  free_matrix(F2,1,3,1,3);
  free_matrix(F1,1,3,1,3);
  free_matrix(F,1,3,1,3);
  free(logc7);
  free(logcn);

  if(t) free(t);
  if(verb) free(verb);
  if(n_flag) free(n_flag);
  if(mode) free(mode);
  if(stop) free(stop);

  //    return 0;
  return(minepsall);
}