 /*
  * Khoros: $Id: lvrmatch.c,v 1.2 1991/12/18 09:26:12 dkhoros Exp $
  */

#if !defined(lint) && !defined(SABER)
static char rcsid[] = "Khoros: $Id: lvrmatch.c,v 1.2 1991/12/18 09:26:12 dkhoros Exp $";
#endif

 /*
  * $Log: lvrmatch.c,v $
 * Revision 1.2  1991/12/18  09:26:12  dkhoros
 * HellPatch3
 *
  */

/*
 *----------------------------------------------------------------------
 *
 * Copyright 1991, University of New Mexico.  All rights reserved.
 * Permission to copy and modify this software and its documen-
 * tation only for internal use in your organization is hereby
 * granted, provided that this notice is retained thereon and
 * on all copies.  UNM makes no representations as to the sui-
 * tability and operability of this software for any purpose.
 * It is provided "as is" without express or implied warranty.
 * 
 * UNM DISCLAIMS ALL WARRANTIES WITH REGARD TO THIS SOFTWARE,
 * INCLUDING ALL IMPLIED WARRANTIES OF MERCHANTABILITY AND FIT-
 * NESS.  IN NO EVENT SHALL UNM BE LIABLE FOR ANY SPECIAL,
 * INDIRECT OR CONSEQUENTIAL DAMAGES OR ANY OTHER DAMAGES WHAT-
 * SOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER
 * IN AN ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS
 * ACTION, ARISING OUT OF OR IN CONNECTION WITH THE USE OR PER-
 * FORMANCE OF THIS SOFTWARE.
 * 
 * No other rights, including, for example, the right to redis-
 * tribute this software and its documentation or the right to
 * prepare derivative works, are granted unless specifically
 * provided in a separate license agreement.
 *---------------------------------------------------------------------
 */

#include "unmcopyright.h"        /* Copyright 1991 by UNM */

/*>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>  <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
 >>>>
 >>>>         File Name: lvrmatch.c
 >>>>
 >>>>      Program Name: vrmatch
 >>>>
 >>>> Date Last Updated: Sat Dec 14 18:11:10 1991 
 >>>>
 >>>>          Routines: lvrmatch - the library call for vrmatch
 >>>>
 >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>   <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<*/


#include "vinclude.h"


/* -library_includes */
#include "vipl/lvshape.h"
/* -library_includes_end */


/****************************************************************
*
* Routine Name: lvrmatch - library call for vrmatch
*
* Purpose:
*    
*    Matching of regions in 2 images
*    
*    

* Input:
*    
*         ima1 _ xvimage structure : First Input Image to be matched.
*    
*         ima2 _ xvimage structure : Second Input Image to be matched.
*    
*    

* Output:
*    
*         image holds the result (matched regions).
*    
*    

*
* Written By: Pascal Adam
*    
*    

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


/* -library_def */
int  lvrmatch(ima1,ima2,image)
struct xvimage *ima1, *ima2, **image;
/* -library_def_end */

/* -library_code */
{
#define VABS(x) ( x > 0 ? x : ( -1.0 * x) )
#define MIN_SIZE 50

int     numreg1, 
        numreg2,
        dim,
        dim_prob,
        i,
        imax,
        j,
        jmax,
        k,
        kmax,
        kk,
        kkk,
        nc,
        nr,
        ind,
        *theta_h,
        maxh;

struct xvimage *createimage(), 
               *imagef;

XVSHAPE **obj1, 
        **obj2;

float   *prob, 
        max,
        maxprob,
        theta_max,
        *theta,
        *len,
        len_mean,
        ratio;

int     *vx, 
        *vy,
        *im1, 
        *im2,
        *imf, 
        *look1,
        *look2,
        num1, 
        num2;

unsigned char *comp,
              *listcomp1, 
              *listcomp2;




   im1 = (int *) ima1 -> imagedata;
   im2 = (int *) ima2 -> imagedata;

   nc  = ima1 -> row_size;
   nr  = ima1 -> col_size;
   dim = nc * nr;

   /* search for the number of region in both images */
   numreg1 = numreg2 = 0;
   for (i = 0; i < dim; i++)
   {
         if (numreg1 < im1[i]) numreg1 = im1[i];
         if (numreg2 < im2[i]) numreg2 = im2[i];
    }

    numreg1 += 1; numreg2 += 1;

    dim_prob = numreg1 * numreg2;

    /* allocate memory for numreg XVSHAPE pointed by obj ... */
    obj1 = (XVSHAPE  **) malloc(sizeof(XVSHAPE *)*numreg1);
    obj2 = (XVSHAPE  **) malloc(sizeof(XVSHAPE *)*numreg2);
    if ((obj1 == NULL) || (obj2 == NULL)) 
    {
       (void)fprintf(stderr,
       "lvrmatch:  No Enough Space For obj _ malloc failed! \n");
       return(0);
    }
    for (i = 0; i < numreg1; i++) 
    {
       obj1[i] = (XVSHAPE *)malloc(sizeof(XVSHAPE));
       if (obj1[i] == NULL) 
       {
          (void)fprintf(stderr,
          "lvrmatch:  No space for obj1[%d] _ malloc failed! \n",i);
          return(0);
       }
       /* initialisation ............................................. */
       obj1[i]->right  =  0;
       obj1[i]->bottom = nr;
       obj1[i]->left   = nc;
       obj1[i]->top    =  0;
       obj1[i]->m00    =  0;
       obj1[i]->m01    =  0;
       obj1[i]->m10    =  0;
    }
    for (i = 0; i < numreg2; i++) 
    {
       obj2[i] = (XVSHAPE *)malloc(sizeof(XVSHAPE));
       if (obj2[i] == NULL) 
       {
          (void)fprintf(stderr,
          "lvrmatch:  No space for obj2[%d] _ malloc failed! \n",i);
          exit(1);
       }
       /* initialisation ............................................. */
       obj2[i]->right  =  0;
       obj2[i]->bottom = nr;
       obj2[i]->left   = nc;
       obj2[i]->top    =  0;
       obj2[i]->m00    =  0;
       obj2[i]->m01    =  0;
       obj2[i]->m10    =  0;
    }

    /* scan the image and compute mpq for each region for cluster  */
    for (i = 0; i < nr; i++)
    {
       for (j = 0; j < nc; j++)
       {
          obj1[ im1[i*nc+j] ]->m00 += (double)1;
          obj1[ im1[i*nc+j] ]->m01 += (double)j;
          obj1[ im1[i*nc+j] ]->m10 += (double)i; 

          obj2[ im2[i*nc+j] ]->m00 += (double)1;
          obj2[ im2[i*nc+j] ]->m01 += (double)j;
          obj2[ im2[i*nc+j] ]->m10 += (double)i;
       }
    }
   
    /* computes area, x_centroid, y_centroid ... */
    for (i = 0; i < numreg1; i++)
    {
       obj1[i]->area =obj1[i]->m00;
       obj1[i]->xcent=obj1[i]->m01/obj1[i]->m00;
       obj1[i]->ycent=obj1[i]->m10/obj1[i]->m00;
    }
    for (i =0; i < numreg2; i++)
    {
       obj2[i]->area =obj2[i]->m00;
       obj2[i]->xcent=obj2[i]->m01/obj2[i]->m00;
       obj2[i]->ycent=obj2[i]->m10/obj2[i]->m00;
    }



    /* allocate memory for array proba[obj1 = obj2] ... */
    prob = (float *) malloc(sizeof(float) * dim_prob);
    if (prob == NULL)
    {
       (void)fprintf(stderr,
       "lvrmatch:  Not Enough Space For Probability _ malloc failed! \n");
       return(0);
    }
    for (i = 0; i < dim_prob; i++) prob[i] = 0.0;
      
    /* allocate memory for  vx and vy (dist centroid obj1[i],obj2[j]) */
    vx = (int *) malloc(sizeof(int) * numreg1 * numreg2);
    vy = (int *) malloc(sizeof(int) * numreg1 * numreg2);
    if ( (vx == NULL) || (vy == NULL))
    {
       (void)fprintf(stderr,
       "lvrmatch:  Not Enough Space For Inter-object centroid distance ! \n");
       return(0);
    }
    /* initialize vx and xy */
    for (i = 0 ; i < numreg1; i++)
    {
       for (j = 0; j < numreg2; j++)
       {
          vx[i* numreg2 +j] = (int)(obj2[j]->xcent - obj1[i]->xcent);
          vy[i* numreg2 +j] = (int)(obj2[j]->ycent - obj1[i]->ycent);
       }
    }

    /* theta will be the angle of the vector (vx,vy) and len will be its length */
    theta = (float *) malloc(sizeof(float) * numreg1 * numreg2);
    len   = (float *) malloc(sizeof(float) * numreg1 * numreg2);
    if ( (theta == NULL) || (len == NULL))
    {
       (void)fprintf(stderr,
       "lvrmatch:  Not Enough Space For Inter-object centroid distance ! \n");
       return(0);
    }


    for (i = 0 ; i < numreg1*numreg2; i++)
    {
       if (vx[i] == 0.0 && vy[i] == 0.0) theta[i] = 0.0;
       else theta[i]=(float)atan2((double)vy[i],(double)vx[i])*180.0/3.14;
 
       if (theta[i] < 0) theta[i] += 360.0;

       len[i]   = (float) sqrt((double)(vx[i]*vx[i] + vy[i]*vy[i]));
    }


    /* initialize the array giving the potential of two objects   */
    /* of being the same. This is based on area and position com- */
    /* raison. To be more precise, for each object or region of   */
    /* the first image, we will create a array saying to which    */
    /* object in the second image it can be compare.............. */
    /* this process is done as well for the objects of the second */
    /* image relatively to the one of the first image.            */
    /* This pre-matching process avoid wasting too much time com- */
    /* paring objects that are very diferent in term of size, or  */
    /* position ................................................. */
    comp = (unsigned char *)malloc(sizeof(unsigned char)*numreg1*numreg2);
    listcomp1 = (unsigned char *)malloc(sizeof(unsigned char)*numreg1*numreg2);
    listcomp2 = (unsigned char *)malloc(sizeof(unsigned char)*numreg1*numreg2);
    if ( (comp == NULL) || (listcomp1 == NULL) || (listcomp2 == NULL) )
    {
       (void)fprintf(stderr,
       "lvrmatch:  Not Enough Space For Inter-object comparaison array ! \n");
       return(0);
    }


    /* ATTENTION to compare surfaces we assume that the epipolar lines are  */
    /* almost horizontal and that the translation due to the difference of  */
    /* position in the left and right images is limited proportionally  to  */
    /* number of columns .... That explains the test vx > nc/6 and vy > 20  */
    /* WE ASSUME as well that the REGION MINIMUM SIZE has to be  at  least  */
    /* MIN_SIZE (cf define above) IF YOU WANT TO GET RID OF THIS VALUE FEEL */
    /* FREE TO DO IT ...................................................... */
    for (i=0; i<numreg1;i++)
    {
       for (j=0; j<numreg2;j++)
       {
          ind = i*numreg2+j;

          ratio = (float)obj1[i]->area > (float)obj2[j]->area ?
                  (float)obj1[i]->area / (float)obj2[j]->area :
                  (float)obj2[j]->area / (float)obj1[i]->area ;


          if (((float)ratio > (float)1.66) ||
              ((float)obj1[i]->area < (float)MIN_SIZE) ||
              ((float)obj2[j]->area < (float)MIN_SIZE) )
             comp[ind] = 0;
          else 
          {
             if ((int)VABS(vx[ind]) > nc/6 || (int)VABS(vy[ind]) > nr/20 ) 
                comp[ind] = 0;
             else comp[ind] = 1;
          }
        }
     }

    for (i=0; i<numreg1;i++)
    {
       k = 0; 
       for (j=0;j<numreg2;j++) 
          if (comp[i*numreg2+j]==1) listcomp1[i*numreg2+k++]=j;

       for (k=k;k<numreg2;k++) listcomp1[i*numreg2+k]=numreg2;
    }

    for (j=0; j<numreg2;j++)
    {
       k = 0;
       for (i=0;i<numreg1;i++)
          if (comp[i*numreg2+j]==1) listcomp2[j+numreg2*k++]=i;

       for (k=k;k<numreg1;k++) listcomp2[j+numreg2*k]=numreg1;
    }

    /* scan first image and compute SXOR(obj1[i],obj2[j])         */
    /* SXOR is define in the following manner :                   */
    /*  SXOR(obj1[i],obj2[j]) = XOR [ Trans(i,j,obj1[i]), obj2[j] */
    /*  with :                                                    */
    /*  Trans(i,j,obj1[i]) is the translation of the obj1[i] such */
    /*  that the centroid of obj1[i] coincide with the centroid   */
    /*  of obj2[j] .............................................. */
    for (i = 0; i < nr; i++)
    { 
       for (j = 0; j < nc; j ++)
       {
          num1 = im1[i*nc +j];
          k = 0; 
          while((kkk=listcomp1[num1*numreg2+k++]) != numreg2)
          {
             kk = num1 *numreg2 +kkk;
             ind = (i+vy[kk]) *nc +j +vx[kk];
             if ( ((i+vy[kk]) < 0) || ((i+vy[kk]) > nr) ||
                  ((j+vx[kk]) < 0) || ((j+vx[kk]) > nc)  ) 
                prob[kk] += 1.0;
             else if ( im2[ind] != kkk)    prob[kk] += 1.0;
          } 


          num2 = im2[i*nc +j];
          k = 0;
          while((kkk=listcomp2[num2+numreg2*k++]) != numreg1)
          {
             kk = num2 + kkk * numreg2;
             ind = (i-vy[kk]) * nc + j-vx[kk];
             if ( ((i-vy[kk]) < 0) || ((i-vy[kk]) > nr) ||
                  ((j-vx[kk]) < 0) || ((j-vx[kk]) > nc)  )
                prob[kk] += 1.0;
             else if (im1[ind] != kkk) prob[kk] += 1.0;
          }
       }
    }

    free(listcomp1); free(listcomp2);

    /* final calculus of probability */
    for (i=0;i<numreg1;i++)
    {
       for (j=0;j<numreg2;j++)
       {
          if (comp[i*numreg2+j] == 0) prob[i*numreg2+j] = 0.0;
          else
          {
             prob[i*numreg2+j] /= (float)(-1.0*(obj1[i]->area+obj2[j]->area));
             prob[i*numreg2+j] += 1.0;
          }
       }
    }
  
    for (i=0;i<numreg1*numreg2;i++)
    {
       if (prob[i] < 0.80) prob[i]=0.0;
    }



    for (i=0;i< numreg1;i++)
    { 
       max = 0.0;
       for (j=0;j<numreg2;j++) 
       {
          if (prob[i*numreg2+j] > max) 
          {
             jmax = j;
             max = prob[i*numreg2+j];
          }
       }
       for (j=0;j<jmax;j++) prob[i*numreg2+j]=0.0;
       for (j=jmax+1;j<numreg2;j++) prob[i*numreg2+j]=0.0;
    }
  
    for (j=0;j<numreg2;j++)
    {
       max = 0.0;
       for (i=0;i<numreg1;i++)
       {
          if (prob[i*numreg2+j] > max)
          {
             imax = i;
             max = prob[i*numreg2+j];
          }
       }

       for (i=0;i<imax;i++) prob[i*numreg2+j]=0.0;
       for (i=imax+1;i<numreg1;i++) prob[i*numreg2+j]=0.0;
    }

/* NOT USE RIGHT NOW !!! NEEDS IMPROOVEMENT .........................*/
    /* compute histograms of direction and length of object that  */
    /* may be match) that will give the direction of the equipolar*/
    /* in its maximum ............................................*/

    /* this first test is use for the stupid case but still possible */
    /* where the user is giving the same segmented image as input    */
    /* this case is particular because it implies to stop right away */
    /* after this OTHERWISE the heuristic working on distances and   */
    /* angles will be lost ......................................... */
/*
    k = 0; len_mean = 0.0;
    for (i=0;i<numreg1*numreg2;i++)
    {
       if (prob[i] != 0.0) 
       {
          k ++;
          len_mean += len[i];
       }
    }
    len_mean /= k;
    
    if (len_mean != 0.0)
    {
     
       theta_h = (int *) malloc(sizeof(int) * 360);
       for (i=0; i<360; i++) theta_h[i] = 0;
    
       for (i=0;i<numreg1*numreg2;i++)
          if ((prob[i] != 0.0) && (len[i] > 9.0))
             theta_h[(int)theta[i]] += 1;
*/
       /* do an average of the histogram on theta (smoothering) */
       /* then  search for peak value of the histogram  to find */
       /* the epipolar direction............................... */
/*
       kmax = 0;
       for (i = 0; i<360;i++)
       {
         k = 0; 
         for (j = -2; j<2; j++)
         {
            ind = i+j;
            if (ind < 0  ) ind += 360;
            if (ind > 360) ind -= 360; 
            k += theta_h[ind];
         }
         if (k > kmax) 
         {
            theta_max = (float)i;
            kmax = k;
         }
       }
       k = 0; len_mean = 0.0;
       for (i=0;i<numreg1*numreg2;i++)
       {
          
          if ((prob[i] != 0.0) && 
              (len[i]  > 9.0)  &&
              ((VABS(theta[i]-theta_max) < 5.0) || (VABS(theta[i]-theta_max) > 355) ) 
          {
             k ++;
             len_mean += len[i];
          }
       }
       len_mean /= k; 
       if (len_mean < 9.0) len_mean = 9.0;


       for (i=0;i< numreg1*numreg2;i++)
          if ( (prob[i] != 0.0) && (len[i] >= len_mean * 1.6) )
             prob[i] = 0.0;
    }
    
*/
    /* look1[i] is the object number in the image 2 that corresponds to  */
    /* the object i of the image 1 if there is one ....................  */
    /* Same thing for look2 ...........................................  */
    look1 = (int *) malloc(sizeof(int) * numreg1);
    look2 = (int *) malloc(sizeof(int) * numreg2);
    if ( (look1 == NULL) || (look2  == NULL))
    {
       (void)fprintf(stderr,
       "lvrmatch:  Not Enough Space For Inter-object centroid distance ! \n");
       return(0);
    }

    for (i=0;i<numreg1;i++) look1[i] = 0;
    for (j=0;j<numreg2;j++) look2[j] = 0;

    k = 0;
    for (i=0;i<numreg1;i++) 
       for (j=0;j<numreg2;j++)
          if (prob[i*numreg2+j] != 0.0) look1[i]=look2[j]=k++;
       
    /* initialize result image structure */
    *image = createimage(nr,
                         2*nc,
                         VFF_TYP_4_BYTE,
                         1,
                         1,
                         '\0',
                         0,
                         0, 
                         VFF_MS_NONE,
                         VFF_MAPTYP_NONE,
                         VFF_LOC_IMPLICIT,
                         0);
   if (*image == NULL)
   {
      (void) fprintf(stderr,
             "lvrmatch: unable to allocate memory for output image!\n\n");
      return(0);
   }
   
   imagef = *image;
   imf = (int *) imagef -> imagedata;
   for (i = 0; i < nr; i++) 
   {
      for (j = 0; j < nc; j++)
      {
         imf[i*2*nc+j]    = look1[im1[i*nc+j]];
         imf[i*2*nc+j+nc] = look2[im2[i*nc+j]];
      }
   }
   return(1);
}
/* -library_code_end */
