text: //
  // 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>
  
  #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;
    }
  }
  
  /*-------------------- 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];
  		b = (double) F[2][1]*x+(double) F[2][2]*y+(double) F[2][3];
  		c = (double) F[3][1]*x+(double) F[3][2]*y+(double) F[3][3];
  		d = (a*(double) p2[i*2]+b*(double) p2[i*2+1]+c);
  		e[i*2] = (d*d)/(a*a+b*b); 
  		  	  
  		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)
  {
   //   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);
        
  		
  	
  
  	/* 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);
     
  
  
  
      /* 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);
  }
  