sbinfilt.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.
**
*/
#ifndef __SBINFILT
#define __SBINFILT

#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include "image_buffer.h"
#include "sbinfilt.h"

#define PI 3.141592653589793143328792211

/*
 *  Binfilt2d function
 *
 *  Allocates space and returns an array
 *  containing an outer product of two
 *  binomial filters.
 *
 */
int *binfilt2d(int order) {
int *w1,*w2,*wtmp,*w3;
int k,l;
  w1=(int *)calloc(order,sizeof(int));
  w2=(int *)calloc(order,sizeof(int));
  w3=(int *)calloc(order*order,sizeof(int));
  w1[0]=1;
  w2[0]=1;
  for(k=0;k<order-1;k++) {
    for(l=1;l<order;l++) {
      w2[l]=w1[l-1]+w1[l];
    }
    wtmp = w1;
    w1 = w2;
    w2 = wtmp;
  }
  for(k=0;k<order;k++) {
    for(l=0;l<order;l++) {
      w3[k+l*order]=w1[k]*w1[l];
    }
  }
  free(w1);
  free(w2);
  return(w3);
}
/*
** Selective binomial filtering routine
** bf_im0       Input image buffer
** bf_ic0       Input confidence buffer
** bf_im2       result image buffer
** bf_ic2       result confidence buffer
** dmax2        Squared max property distance
** cmin         Weighted fraction of pixels reqired for c=1
** roi_side     2 -> 2x2, 4->4x4 6-> 6x6
** miter        Number of M-estimation steps to follow.
**
*/
void sbinfilt2d(buffer *bf_im0,ibuffer *bf_ic0,buffer *bf_im1,
		ibuffer *bf_ic1,fpnum dmax2,fpnum cmin,
		int roi_side,int miter)
{
int x,y,k,l,m,cind,rind,stepsz,donefl,olcnt;
int xx,yy,offset;
int rows,cols,pixels,ndim;
int rows2,cols2,pixels2;
fpnum icsum,wsum;
fpnum *im_roi,*imr,*wi;
int *w,*ic_roi;
fpnum cdiff,cdist,max_dist;
int max_ind,roisz,ul,ll;
fpnum *im0data,*im1data;
int *ic0data,*ic1data;

  stepsz=2;
  /* Input arrays */
  im0data=bf_im0->data;
  ic0data=bf_ic0->data;
  rows=bf_im0->rows;
  cols=bf_im0->cols;
  ndim=bf_im0->ndim;
  pixels=rows*cols;

  /* Output arrays */
  im1data=bf_im1->data;
  ic1data=bf_ic1->data;
  rows2=bf_im1->rows;
  cols2=bf_im1->cols;
  pixels2=rows2*cols2;

  if((roi_side%2) !=0) {
    printf("Error wrong roi_side value %d (not 2,4,6)\n",roi_side);
    return;
  }

  /* Spatial weights for kernel */
  w=binfilt2d(roi_side);
  wsum=exp(2*(roi_side-1)*log(2));  /* Weight sum = 2^(2*(n-1)) */
  roisz=roi_side*roi_side;

  /* Find limits of kernel loop */
  ll=(int)-floor((roi_side-1)/2);
  ul=(int)floor((roi_side+2)/2);


  im_roi = (fpnum *)calloc(ndim*roisz,sizeof(fpnum));/* four pixels */
  ic_roi = (int *)calloc(roisz,sizeof(int));         /* certainties */
  wi     = (fpnum *)calloc(roisz,sizeof(fpnum));     /* weights */
  imr    = (fpnum *)calloc(ndim,sizeof(fpnum));      /* Result pixel */
  for(x=0;x<cols;x+=stepsz) { 
    for(y=0;y<rows;y+=stepsz) { 
      rind=y+x*rows;
      /* Cell order: */
      /*    1 3      */
      /*    2 4      */
      /* right-down steps. vN-Valid flags, oN-offsets for cell N */
      l=0;
      for(xx=ll;xx<ul;xx++) {
	for(yy=ll;yy<ul;yy++) {
	  ic_roi[l]=0;            /* Assume c=0 */
	  for(k=0;k<ndim;k++) {
	    im_roi[l+k*roisz]=0;  /* Assume p=0 */
	  }
	  /* Check if inside image */
	  if((x+xx>=0)&&(x+xx<cols)&&(y+yy>=0)&&(y+yy<rows)) {
	    offset=yy+xx*rows;
	    ic_roi[l]=ic0data[rind+offset];           /* Read c */
	    for(k=0;k<ndim;k++) {
	      cind=rind+k*pixels;
	      im_roi[l+k*roisz]=im0data[cind+offset]; /* Read p */
	    }
	  }
	  l++;
	}
      }

      /*
       *  Find approximative cluster centre using
       *  one-by-one outlier rejection.
       */

      /* First hypothesis -- all cluster */
      olcnt=0;  /* Number of outliers */
      for(l=0;l<roisz;l++) wi[l]=1;
      do {
	donefl=1;  /* Assume OK until failure :-) */
	/* Count inliers */
	icsum=0;for(l=0;l<roisz;l++) icsum+=(fpnum)ic_roi[l]*w[l]*wi[l];
	if(icsum>0) {
	  /* Compute average/cluster centre */
	  for(k=0;k<ndim;k++) {
	    imr[k]=0;
	    for(l=0;l<roisz;l++)
	      imr[k]+=(fpnum)ic_roi[l]*im_roi[l+k*roisz]*w[l]*wi[l];
	    imr[k]/=icsum;
	  }
	  /* Find highest distance to cluster */
	  max_dist=0;max_ind=-1;
	  for(l=0;l<roisz;l++) {
	    cdist=0;
	    if(ic_roi[l]*wi[l]) {
	      for(k=0;k<ndim;k++) {
		cdiff = im_roi[l+k*roisz]-imr[k];
		cdist +=cdiff*cdiff;
	      }
	    }
	    if(cdist>max_dist) {
	      max_dist = cdist;
	      max_ind  = l;
	    }
	  }
	  /* Reject largest outlier */
	  if(max_dist>=dmax2) {
	    wi[max_ind]=0;
	    olcnt++;   /* Another outlier bites the dust */
	    donefl=0;
	  }
	}
      } while(!donefl);

      /*
       *  Refine cluster position using
       *  iterative reweighted least squares.
       */
      if(icsum>0) {
	for(m=0;m<miter;m++) {
	  /* Compute weights */
	  icsum=0;
	  for(l=0;l<roisz;l++) {
	    cdist=0;wi[l]=0;
	    if(ic_roi[l]) {
	      for(k=0;k<ndim;k++) {
		cdiff = im_roi[l+k*roisz]-imr[k];
		cdist +=cdiff*cdiff;
	      }
	      if(cdist<dmax2) {
		wi[l]=cos(sqrt(PI*PI/4/dmax2*cdist));
		wi[l]=wi[l]*wi[l];
		icsum+=w[l]*wi[l];
	      } else {
		wi[l]=0;
	      }
	    }
	  }
	  /* Compute average/cluster centre */
	  for(k=0;k<ndim;k++) {
	    imr[k]=0;
	    for(l=0;l<roisz;l++)
	      imr[k]+=(fpnum)ic_roi[l]*im_roi[l+k*roisz]*w[l]*wi[l];
	    imr[k]/=icsum;
	  }
	}
      }
      /*
       * Store result
       */

      rind=(y+x*rows2)/stepsz; /* Indexing in out image */
      ic1data[rind]=(icsum>=wsum*cmin);
      for(k=0;k<ndim;k++) {
	im1data[rind+k*pixels2]=imr[k];
      }
    }
  }
  free(im_roi);
  free(ic_roi);
  free(imr);
  free(wi);
  free(w);
}
/*
** Selective binomial filtering routine
** bf_im0       Input image buffer
** bf_ic0       Input certainty buffer
** bf_im1       result image buffer
** bf_ic1       result certainty buffer
** dmax2        Squared max property distance
** cmin         Weighted fraction of pixels reqired for c=1
** miter        Number of M-estimation steps to follow.
**
*/
void sbinfilt2d12(buffer *bf_im0,ibuffer *bf_ic0,buffer *bf_im1,
		  ibuffer *bf_ic1,fpnum dmax2,fpnum cmin,int miter)
{
int x,y,k,l,m,cind,rind,stepsz;
int xx,yy,offset;
int rows,cols,pixels,ndim;
int rows2,cols2,pixels2;
fpnum icsum;
int *ic_roi;
fpnum *im_roi,*imr,*wi;
fpnum cdiff,cdist,max_dist;
int max_ind;
fpnum *im0data,*im1data;
int *ic0data,*ic1data;
int xxl[] = {-1,-1,0,0,0,0,1,1,1,1,2,2};
int yyl[] = {0,1,-1,0,1,2,-1,0,1,2,0,1};
int roisz = 12; /* Number of pixels in roi */

  stepsz=2;
  /* Input arrays */
  im0data=bf_im0->data;
  ic0data=bf_ic0->data;
  rows=bf_im0->rows;
  cols=bf_im0->cols;
  ndim=bf_im0->ndim;
  pixels=rows*cols;

  /* Output arrays */
  im1data=bf_im1->data;
  ic1data=bf_ic1->data;
  rows2=bf_im1->rows;
  cols2=bf_im1->cols;
  pixels2=rows2*cols2;

  im_roi = (fpnum *)calloc(ndim*roisz,sizeof(fpnum));/* roi pixels */
  ic_roi = (int *)calloc(roisz,sizeof(int));         /* certainties */
  wi     = (fpnum *)calloc(roisz,sizeof(fpnum));     /* weights */
  imr    = (fpnum *)calloc(ndim,sizeof(fpnum));      /* Result pixel */
  for(x=0;x<cols;x+=stepsz) { 
    for(y=0;y<rows;y+=stepsz) { 
      rind=y+x*rows;
      /* Cell order: */
      /*    1 3      */
      /*    2 4      */
      /* right-down steps. vN-Valid flags, oN-offsets for cell N */
      for(l=0;l<roisz;l++) {
	xx=xxl[l];
	yy=yyl[l];
	ic_roi[l]=0;            /* Assume c=0 */
	for(k=0;k<ndim;k++) {
	  im_roi[l+k*roisz]=0;  /* Assume p=0 */
	}
	/* Check if inside image */
	if((x+xx>=0)&&(x+xx<cols)&&(y+yy>=0)&&(y+yy<rows)) {
	  offset=yy+xx*rows;
	  ic_roi[l]=ic0data[rind+offset];           /* Read c */
	  for(k=0;k<ndim;k++) {
	    cind=rind+k*pixels;
	    im_roi[l+k*roisz]=im0data[cind+offset]; /* Read p */
	  }
	}
      }

      /*
       *  Find approximative cluster centre using
       *  one-by-one outlier rejection.
       */

      /* First hypothesis -- all cluster */
      for(l=0;l<roisz;l++) wi[l]=1;
      do {
	/* Count inliers */
	icsum=0;for(l=0;l<roisz;l++) icsum+=(fpnum)ic_roi[l]*wi[l];
	if(icsum>0) {
	  /* Compute average/cluster centre */
	  for(k=0;k<ndim;k++) {
	    imr[k]=0;
	    for(l=0;l<roisz;l++)
	      imr[k]+=(fpnum)ic_roi[l]*im_roi[l+k*roisz]*wi[l];
	    imr[k]/=icsum;
	  }
	  /* Find highest distance to cluster */
	  max_dist=0;max_ind=-1;
	  for(l=0;l<roisz;l++) {
	    cdist=0;
	    if(ic_roi[l]*wi[l]) {
	      for(k=0;k<ndim;k++) {
		cdiff = im_roi[l+k*roisz]-imr[k];
		cdist +=cdiff*cdiff;
	      }
	    }
	    if(cdist>max_dist) {
	      max_dist = cdist;
	      max_ind  = l;
	    }
	  }
	  /* Reject largest outlier */
	  if(max_dist>=dmax2) {
	    wi[max_ind]=0;   /* Another outlier bites the dust */
	  }
	}
      } while(max_dist>=dmax2);

      /*
       *  Refine cluster position using
       *  iterative reweighted least squares.
       */
      if(icsum>0) {
	for(m=0;m<miter;m++) {
	  /* Compute weights */
	  icsum=0;
	  for(l=0;l<roisz;l++) {
	    cdist=0;wi[l]=0;
	    if(ic_roi[l]) {
	      for(k=0;k<ndim;k++) {
		cdiff = im_roi[l+k*roisz]-imr[k];
		cdist +=cdiff*cdiff;
	      }
	      if(cdist<dmax2) {
		wi[l]=cos(sqrt(PI*PI/4/dmax2*cdist));
		wi[l]=wi[l]*wi[l];
		icsum+=wi[l];
	      } else {
		wi[l]=0;
	      }
	    }
	  }
	  /* Compute average/cluster centre */
	  for(k=0;k<ndim;k++) {
	    imr[k]=0;
	    for(l=0;l<roisz;l++)
	      imr[k]+=(fpnum)ic_roi[l]*im_roi[l+k*roisz]*wi[l];
	    imr[k]/=icsum;
	  }
	}
      }
      /*
       * Store result
       */

      rind=(y+x*rows2)/stepsz; /* Indexing in out image */
      ic1data[rind]=(icsum>=cmin*roisz);
      for(k=0;k<ndim;k++) {
	im1data[rind+k*pixels2]=imr[k];
      }
    }
  }
  free(im_roi);
  free(ic_roi);
  free(imr);
  free(wi);
}

#endif /* __SBINFILT */