merge_blobs.c

it runs for blob detection with opencv

/*
**  Source file for blobdetect. (c) 2004 Per-Erik Forssen
**
**  This program is free software; you can redistribute it and/or modify
**  it under the terms of the GNU General Public License as published by
**  the Free Software Foundation; either version 2 of the License, or
**  (at your option) any later version.
**  
**  See the file COPYING for details.
**
*/
#include "merge_blobs.h"
#include <stdlib.h>
#include <math.h>

/*
** Blob merging subroutine
**
**  mvec1        Moment vector 1
**  mvec2        Moment vector 2
**  mvecn        Output moment vector
**  pvec1        Property vector 1
**  pvec2        Property vector 2
**  pvecn        Output property vector
**  ndim         Dimensionality of vector field
*/

void merge_blobs(fpnum *mvec1,fpnum *mvec2,fpnum *mvecn,
		 fpnum *pvec1,fpnum *pvec2,fpnum *pvecn,
		 int ndim)
{
  int d;
  fpnum atot,mx,my,ixx,ixy,iyy;
  atot=mvec1[0]+mvec2[0];
  mx=(mvec1[1]*mvec1[0]+mvec2[1]*mvec2[0])/atot;
  my=(mvec1[2]*mvec1[0]+mvec2[2]*mvec2[0])/atot;
  /* Inertia about (0,0) */
  ixx =((mvec1[3]+mvec1[1]*mvec1[1])*mvec1[0]+
	(mvec2[3]+mvec2[1]*mvec2[1])*mvec2[0])/atot;
  ixy =((mvec1[4]+mvec1[1]*mvec1[2])*mvec1[0]+
	(mvec2[4]+mvec2[1]*mvec2[2])*mvec2[0])/atot;
  iyy =((mvec1[5]+mvec1[2]*mvec1[2])*mvec1[0]+
	(mvec2[5]+mvec2[2]*mvec2[2])*mvec2[0])/atot;
  /* Inertia about (mx,my) */
  ixx=ixx-mx*mx;
  ixy=ixy-mx*my;
  iyy=iyy-my*my;
  
  /* Store in mvecn,pvecn */

  mvecn[0]=atot;
  mvecn[1]=mx;
  mvecn[2]=my;
  mvecn[3]=ixx;
  mvecn[4]=ixy;
  mvecn[5]=iyy;
  
  for(d=0;d<ndim;d++) {
    pvecn[d]=(pvec1[d]*mvec1[0]+pvec2[d]*mvec2[0])/atot;
  }
}
/*
** Main bloblist_merge_cnt function
**
** bf_mvec0    Array of input moment vectors
** bf_pvec0    Array of input property vectors
** bf_mvecn    Array of output moment vectors
** bf_pvecn    Array of output property vectors
** bf_out_ind  Array of index pointers
** cntl        Overlap count list
** minc        Merger threshold
**
** Returns number of merged regions
*/
int bloblist_merge_cnt(buffer *bf_mvec0,buffer *bf_pvec0,buffer *bf_mvecn,
		       buffer *bf_pvecn,ibuffer *bf_out_ind,ibuffer *bf_cntl,
		       fpnum minc)
{
  int k,l,d,cind,m1,m2,mh,ml,mregions;
  int ndim,regions;
  fpnum amin;
  fpnum *mvec0,*pvec0,*mvecn,*pvecn;
  fpnum *mvec1,*mvec2,*mvec3,*pvec1,*pvec2,*pvec3;
  int *cntl,*out_ind;

  mvec0   = bf_mvec0->data;
  pvec0   = bf_pvec0->data;
  mvecn   = bf_mvecn->data;
  pvecn   = bf_pvecn->data;
  cntl    = bf_cntl->data;
  out_ind = bf_out_ind->data;

  ndim    = bf_pvec0->rows;
  regions = bf_pvec0->cols;

  mvec3=(fpnum *)calloc(6,sizeof(fpnum));
  pvec3=(fpnum *)calloc(ndim,sizeof(fpnum));

  /* First copy old mvec and pvec, and initialize out_ind */
  for(k=0;k<regions;k++) {
    for(d=0;d<6;d++) {
      mvecn[d+k*6]=mvec0[d+k*6];
    }
    for(d=0;d<ndim;d++) {
      pvecn[d+k*ndim]=pvec0[d+k*ndim];
    }
    out_ind[k]=k;
  }

  /* Merging algorithm */
  for(k=0;k<regions;k++) {
    for(l=k+1;l<regions;l++) {
      cind=l*(l-1)/2+k;
      /* find smallest area of blob k and l */
      amin=mvec0[0+k*6];
      if(amin>mvec0[0+l*6]) amin=mvec0[0+l*6];

      if((cntl[cind]>minc*sqrt(amin))&&(amin>0)) {
	/* Merge blob k and l */

	/* Find merger if already merged */
	m1=k;while(m1!=out_ind[m1]) m1=out_ind[m1];
	m2=l;while(m2!=out_ind[m2]) m2=out_ind[m2];

	if(m1!=m2) {
	  /* Not already merged */
	  mvec1=&mvecn[m1*6];pvec1=&pvecn[m1*ndim];
	  mvec2=&mvecn[m2*6];pvec2=&pvecn[m2*ndim];

	  merge_blobs(mvec1,mvec2,mvec3,pvec1,pvec2,pvec3,ndim);
	  /* Find highest and lowest index */
	  mh=m1;if(m2>mh) mh=m2;
	  ml=m1;if(m2<ml) ml=m2;

	  /* Store result */
	  for(d=0;d<6;d++) {
	    mvecn[d+ml*6]=mvec3[d];    /* Store here */
	    mvecn[d+mh*6]=0;           /* Clear here */
	  }
	  for(d=0;d<ndim;d++) {
	    pvecn[d+ml*ndim]=pvec3[d]; /* Store here */
	    pvecn[d+mh*ndim]=0;        /* Clear here */
	  }
	  /* Point to merged blob */
	  out_ind[mh]=ml;
	  out_ind[k]=ml;
	  out_ind[l]=ml;
	}
      }
    }
  }
  free(mvec3);
  free(pvec3);

  mregions=0;
  for(k=0;k<regions;k++) {
    if(mvecn[0+k*6]>0) mregions++;
  }
  return(mregions);
}
/*
** Main bloblist_merge_cnt2 function
** More costly merge, but better.
**
** bf_mvec0    Array of input moment vectors
** bf_pvec0    Array of input property vectors
** bf_mvecn    Array of output moment vectors
** bf_pvecn    Array of output property vectors
** bf_out_ind  Array of index pointers
** cntl        Overlap count list
** minc        Merger threshold
** dmax2       Squared max property distance
**
** Returns number of merged regions
*/
int bloblist_merge_cnt2(buffer *bf_mvec0,buffer *bf_pvec0,buffer *bf_mvecn,
			buffer *bf_pvecn,ibuffer *bf_out_ind,ibuffer *bf_cntl,
			fpnum minc,fpnum dmax2)
{
  int k,l,d,cind,rind,mh,ml,mregions;
  int ndim,regions,min1,min2,mcind,ncnt;
  fpnum amin,diff,cdist,mindist;
  fpnum *mvec0,*pvec0,*mvecn,*pvecn;
  int *cntl,*out_ind;
  fpnum *mvec1,*mvec2,*mvec3,*pvec1,*pvec2,*pvec3;
  fpnum *dlist;
  buffer *bf_dlist;
  int *mlist;
  ibuffer *bf_mlist;

  mvec0   = bf_mvec0->data;
  pvec0   = bf_pvec0->data;
  mvecn   = bf_mvecn->data;
  pvecn   = bf_pvecn->data;
  cntl    = bf_cntl->data;
  out_ind = bf_out_ind->data;

  ndim    = bf_pvec0->rows;
  regions = bf_pvec0->cols;

  mvec3=(fpnum *)calloc(6,sizeof(fpnum));
  pvec3=(fpnum *)calloc(ndim,sizeof(fpnum));

  /* First copy old mvec and pvec, and initialize out_ind */
  for(k=0;k<regions;k++) {
    for(d=0;d<6;d++) {
      mvecn[d+k*6]=mvec0[d+k*6];
    }
    for(d=0;d<ndim;d++) {
      pvecn[d+k*ndim]=pvec0[d+k*ndim];
    }
    out_ind[k]=k;
  }

  /* Count merging candidates (for allocation) */
  ncnt=0;
  for(k=0;k<regions*(regions-1)/2;k++)
    if(cntl[k]>0) ncnt++;
  
  /* Generate compact relationship list */
  bf_mlist=ibuffer_new(2,ncnt,1);
  mlist=bf_mlist->data;
  /* Generate compact distance list */
  bf_dlist=buffer_new(1,ncnt,1);
  dlist=bf_dlist->data;
  /* Find smallest distance */
  mindist=10000;mcind=0;

  rind=0;
  for(l=1;l<regions;l++) {
    for(k=0;k<l;k++) {
      cind=l*(l-1)/2+k;
      if(cntl[cind]>0) {
	/* find smallest area of blob k and l */
	amin=mvec0[0+k*6];
	if(amin>mvec0[0+l*6]) amin=mvec0[0+l*6];
	if((cntl[cind]>minc*sqrt(amin))&&(amin>0)) {
	  mlist[0+rind*2]=k;
	  mlist[1+rind*2]=l;
	  /* Compute property distance */
	  cdist=0.0;
	  for(d=0;d<ndim;d++) {
	    diff=pvecn[d+k*ndim]-pvecn[d+l*ndim];
	    cdist+=diff*diff;
	  }
	  dlist[rind]=cdist;
	  if(cdist<mindist) {
	    /* Remember smallest distance */
	    mindist=cdist;
	    mcind=rind;
	  }
	  rind++;
	}
      }
    }
  }
/*   mexPrintf("TEST: rind=%d < ncnt=%d ?\n",rind,ncnt); */

  ncnt=rind;  /* Actual number of neighbours */

  /*
   * Merging algorithm:
   * Keep merging regions with property distance
   * below dmax, starting with most similar.
   */
  while(mindist<dmax2) {
    /* Blobs to merge */
    min1=mlist[0+mcind*2];
    min2=mlist[1+mcind*2];

    mvec1=&mvecn[min1*6];pvec1=&pvecn[min1*ndim];
    mvec2=&mvecn[min2*6];pvec2=&pvecn[min2*ndim];      
    merge_blobs(mvec1,mvec2,mvec3,pvec1,pvec2,pvec3,ndim);
    /* Find highest and lowest index */
    if(min1>min2) {mh=min1;ml=min2;} else {mh=min2;ml=min1;}
    
    /* Store result */
    for(d=0;d<6;d++) {
      mvecn[d+ml*6]=mvec3[d];    /* Store here */
      mvecn[d+mh*6]=0;           /* Clear here */
    }
    for(d=0;d<ndim;d++) {
      pvecn[d+ml*ndim]=pvec3[d]; /* Store here */
      pvecn[d+mh*ndim]=0;        /* Clear here */
    }
    
    /* Point to merged blob */
    for(k=0;k<regions;k++) {
      if(out_ind[k]==mh) out_ind[k]=ml;
    }
    
    /* Remove merger candidate */
    mlist[0+mcind*2]=-1;
    mlist[1+mcind*2]=-1;
    dlist[mcind]=2*dmax2; /* Ignore this one from now on */
    
    /* Update mlist and dlist */
    for(rind=0;rind<ncnt;rind++) {
      k=mlist[0+rind*2];
      l=mlist[1+rind*2];
      /* Replace occurrences of mh with ml */
      if(k==mh) { mlist[0+rind*2]=ml;k=ml;}
      if(l==mh) { mlist[1+rind*2]=ml;l=ml;}
      if(k==l) {
	/* Remove if same */
	k=-1;l=-1;
	mlist[0+rind*2]=-1;
	mlist[1+rind*2]=-1;
	dlist[rind]=2*dmax2;
      }
      if((k==ml)||(l==ml)) {
	/* Recompute this distance */
	cdist=0.0;
	for(d=0;d<ndim;d++) {
	  diff=pvecn[d+k*ndim]-pvecn[d+l*ndim];
	  cdist+=diff*diff;
	}
	dlist[rind]=cdist;
      }
    }
    /* Find new smallest distance */
    mindist=10000;mcind=0;
    for(rind=0;rind<ncnt;rind++) {
      if(dlist[rind]<mindist) {
	mindist=dlist[rind];
	mcind=rind;
      }
    }
  }
  
  free(mvec3);
  free(pvec3);
  ibuffer_free(bf_mlist);
  buffer_free(bf_dlist);

  mregions=0;
  for(k=0;k<regions;k++) {
    if(mvecn[0+k*6]>0) mregions++;
  }
  return(mregions);
}
/*
 *  Require that all blobs have det(I)>0 and mvec(0)>amin
 *  Mark invalid blobs by setting mvec(0)=0 and out_ind(k)=0
 *  and count number of valid blobs.
 *
 *  bf_mvec     Moment vectors
 *  bf_out_ind  Array of index pointers
 *  amin        Minimum required area
 *
 */
int bloblist_mark_invalid(buffer *bf_mvec,ibuffer *bf_out_ind,int amin)
{
  int regions,k,validcnt;
  fpnum *mvec;
  int *out_ind;

  mvec    = bf_mvec->data;
  regions = bf_mvec->cols;
  out_ind = bf_out_ind->data;

  validcnt=0;
  for(k=0;k<regions;k++) {
    if(out_ind[k]==k) {
      if((mvec[0+k*6]>amin)&&
	 ((mvec[3+k*6]*mvec[5+k*6]-SQR(mvec[4+k*6]))>2e-9)) {
	validcnt++;
      } else {
	mvec[0+k*6]=0; /* Mark as invalid */
      }
    }
  }
  return(validcnt);
}
/*
 *  Remove holes in bloblists after bloblist_merge_cnt
 *
 *  bf_mvecn    Input moment vectors
 *  bf_pvecn    Input property vectors
 *  bf_cscn     Input detection scales
 *  bf_mvecm    Output moment vectors
 *  bf_pvecm    Output property vectors
 *  bf_cscm     Output detection scales
 *  bf_out_ind  Index pointer list
 *
 */
void  bloblist_compact(buffer *bf_mvecn,buffer *bf_pvecn,ibuffer *bf_cscn,
		       buffer *bf_mvecm,buffer *bf_pvecm,ibuffer *bf_cscm,
		       ibuffer *bf_out_ind)
{
  fpnum *mvecn,*pvecn;
  int *cscn,*cscm,*out_ind;
  fpnum *mvecm,*pvecm;
  int ndim,regions,mregions,k,d,rind;

  cscn     = bf_cscn->data;
  mvecn    = bf_mvecn->data;
  pvecn    = bf_pvecn->data;
  out_ind  = bf_out_ind->data;
  mvecm    = bf_mvecm->data;
  pvecm    = bf_pvecm->data;
  cscm     = bf_cscm->data;

  ndim     = bf_pvecn->rows;
  regions  = bf_pvecn->cols;
  mregions = bf_pvecm->cols;

  rind=0;
  for(k=0;k<regions;k++) {
    /* Eliminate empty blobs and translate to region numbers */
    if((out_ind[k]==k)&&(mvecn[0+k*6]==0)) {
      out_ind[k]=0;
    } else {
      out_ind[k]=out_ind[k]+1; /* Translate to blob number (0 means none) */
    }

    /* Shrink list */
    if(mvecn[0+k*6]>0) {
      for(d=0;d<6;d++) {
	mvecm[d+rind*6]=mvecn[d+k*6];
      }
      for(d=0;d<ndim;d++) {
	pvecm[d+rind*ndim]=pvecn[d+k*ndim];
      }
      cscm[rind]=cscn[k];
      rind++;
    }
  }
}