mathematic.cpp

capture the image of the inpu object from the user and then detect image edge of the object

#include "StdAfx.h"
//#include "Vision.h"
#include "Mathematic.h"

#define pi 3.141592
#define pi1 pi/4
#define pi3 pi1*3

void swap(double &a,double &b)
{
double c;
c=a;
a=b;
b=c;

}
void swap(int &a,int &b)
{
int c;
c=a;
a=b;
b=c;

}
void Sort(double* aa, int num)
{

int i,j;
for(i=0;i<num;i++)
{
  for(j=0;j<=i;j++)
	  if(aa[i]>aa[j])
		  swap(aa[j],aa[i]);
}
}


double NormalizeAngle(double ang)
{
while(ang>=360)
ang-=360;
while(ang<0)
ang+=360;
return ang;
}

double NormalizeAngle2(double ang)
{
while(ang>=180)
ang-=360;
while(ang<-180)
ang+=360;
return ang;
}

double NormalizeDegAngle(double ang)
{
while(ang>=pi)
ang-=pi*2;
while(ang<-pi)
ang+=pi*2;
return ang;
}


double DisFabs(double x1,double y1,
			   double x2,double y2)
{
return(fabs(x1-x2)+fabs(y1-y2));
}

double Dis(double x1,double y1,
			   double x2,double y2)
{
return(sqrt((x1-x2)*(x1-x2)+(y1-y2)*(y1-y2)));
}

BOOL RGB2YUV(BYTE R,BYTE G,BYTE B,int& Y,int& U,int& V)
{


	 Y = int(0.299*R + 0.587*G + 0.114*B + 0.500);
	 U = int(0.565*(B - Y) + 128);
	 V = int(0.713*(R - Y) + 128);

	 return true;
}

int Discretize(double num,short interval)
{
    return(int(num/interval)*interval);

}
double Evaluate(int a,int b)
{
return(double(1/fabs(a-b)));
}

float Angle2Deg(float ang)
{
return((float)(ang*pi/180));
}
float Deg2Angle(float deg)
{
	return((float)(deg*180/pi));
}



/******************************************************/
/*                                                    */
/* This function performs binary smooth operation     */
/* 28-Jan-2000                                        */
/* StrRow: starting row                               */
/* StrCol: starting column                            */
/* EndRow: End row                                    */
/* EndCol: End column                                 */
/* *frm1 : pointer to target image array              */
/* *res : pointer to result image array               */
/*                                                    */
/******************************************************/                

void binarysmooth(int StrRow, int StrCol, int EndRow, int EndCol, int COL, BYTE *frm1, int *res) 
{
#define delpix 20
#define addpix 40
long int i,j;
unsigned char a,b,c,d,e,f,g,h,p;

/*mask1*/ 
for(j=StrRow; j<EndRow; j++){
for(i=StrCol; i<EndCol; i++){
	if(frm1[(j-1)*COL+(i-1)]>0) a=1; else a=0;
	if(frm1[(j-1)*COL+i]>0 ) b=1; else b=0;
	if(frm1[(j-1)*COL+(i+1)]>0)c=1;else c=0;
	if (frm1[j*COL+(i-1)]>0) d=1; else d=0;
	if (frm1[j*COL+i]>0)p=1;else p=0;
	if (frm1[j*COL+(i+1)]>0) e=1;else e=0;
	if (frm1[(j+1)*COL+(i-1)]>0) f=1;else f=0;
	if (frm1[(j+1)*COL+i]>0) g=1;else g=0;
	if (frm1[(j+1)*COL+(i+1)]>0) h=1;else h=0;
if (p||(b&&g&&(d||e))||((d&&e)&&(b||g)) ==TRUE) res [j*COL+i]=addpix;
else res[j*COL+i]=delpix;
}}

for(j=StrRow; j<EndRow; j++){
for(i=StrCol; i<EndCol; i++){
if (res[j*COL+i]==addpix) frm1[j*COL+i]=255;
if (res[j*COL+i]==delpix) frm1[j*COL+i]=0;
}}


/*mask2*/
for(j=StrRow; j<EndRow; j++){
for(i=StrCol; i<EndCol; i++){
	if(frm1[(j-1)*COL+(i-1)]>0) a=1; else a=0;
	if(frm1[(j-1)*COL+i]>0 ) b=1; else b=0;
	if(frm1[(j-1)*COL+(i+1)]>0)c=1;else c=0;
	if (frm1[j*COL+(i-1)]>0) d=1; else d=0;
	if (frm1[j*COL+i]>0)p=1;else p=0;
	if (frm1[j*COL+(i+1)]>0) e=1;else e=0;
	if (frm1[(j+1)*COL+(i-1)]>0) f=1;else f=0;
	if (frm1[(j+1)*COL+i]>0) g=1;else g=0;
	if (frm1[(j+1)*COL+(i+1)]>0) h=1;else h=0;
if (p&&((a||b||d)&&(e||g||h)&&(b||c||e)&&(d||f||g)) ==TRUE) res [j*COL+i]=addpix;
else res[j*COL+i]=delpix;
}}
for(j=StrRow; j<EndRow; j++){
for(i=StrCol; i<EndCol; i++){
if (res[j*COL+i]==addpix) frm1[j*COL+i]=255;
if (res[j*COL+i]==delpix) frm1[j*COL+i]=0;
}}


/*mask3*/
for(j=StrRow; j<EndRow; j++){
for(i=StrCol; i<EndCol; i++){
	if(frm1[(j-1)*COL+(i-1)]>0) a=1; else a=0;
	if(frm1[(j-1)*COL+i]>0 ) b=1; else b=0;
	if(frm1[(j-1)*COL+(i+1)]>0)c=1;else c=0;
	if (frm1[j*COL+(i-1)]>0) d=1; else d=0;
	if (frm1[j*COL+i]>0)p=1;else p=0;
	if (frm1[j*COL+(i+1)]>0) e=1;else e=0;
	if (frm1[(j+1)*COL+(i-1)]>0) f=1;else f=0;
	if (frm1[(j+1)*COL+i]>0) g=1;else g=0;
	if (frm1[(j+1)*COL+(i+1)]>0) h=1;else h=0;
if ((!p&&(d&&f&&g)&&!(a||b||c||e||h)||p) ==TRUE) res [j*COL+i]=addpix;
else res[j*COL+i]=delpix;
}}
for(j=StrRow; j<EndRow; j++){
for(i=StrCol; i<EndCol; i++){
if (res[j*COL+i]==addpix) frm1[j*COL+i]=255;
if (res[j*COL+i]==delpix) frm1[j*COL+i]=0;
}}

/*mask4*/
for(j=StrRow; j<EndRow; j++){
for(i=StrCol; i<EndCol; i++){
	if(frm1[(j-1)*COL+(i-1)]>0) a=1; else a=0;
	if(frm1[(j-1)*COL+i]>0 ) b=1; else b=0;
	if(frm1[(j-1)*COL+(i+1)]>0)c=1;else c=0;
	if (frm1[j*COL+(i-1)]>0) d=1; else d=0;
	if (frm1[j*COL+i]>0)p=1;else p=0;
	if (frm1[j*COL+(i+1)]>0) e=1;else e=0;
	if (frm1[(j+1)*COL+(i-1)]>0) f=1;else f=0;
	if (frm1[(j+1)*COL+i]>0) g=1;else g=0;
	if (frm1[(j+1)*COL+(i+1)]>0) h=1;else h=0;
if ((!p&&(a&&b&&d)&&!(c||e||f||g||h)||p) ==TRUE) res [j*COL+i]=addpix;
else res[j*COL+i]=delpix;
}}
for(j=StrRow; j<EndRow; j++){
for(i=StrCol; i<EndCol; i++){
if (res[j*COL+i]==addpix) frm1[j*COL+i]=255;
if (res[j*COL+i]==delpix) frm1[j*COL+i]=0;
}}

/*mask5*/
for(j=StrRow; j<EndRow; j++){
for(i=StrCol; i<EndCol; i++){
	if(frm1[(j-1)*COL+(i-1)]>0) a=1; else a=0;
	if(frm1[(j-1)*COL+i]>0 ) b=1; else b=0;
	if(frm1[(j-1)*COL+(i+1)]>0)c=1;else c=0;
	if (frm1[j*COL+(i-1)]>0) d=1; else d=0;
	if (frm1[j*COL+i]>0)p=1;else p=0;
	if (frm1[j*COL+(i+1)]>0) e=1;else e=0;
	if (frm1[(j+1)*COL+(i-1)]>0) f=1;else f=0;
	if (frm1[(j+1)*COL+i]>0) g=1;else g=0;
	if (frm1[(j+1)*COL+(i+1)]>0) h=1;else h=0;
if ((!p&&(e&&g&&h)&&!(a||b||c||d||f)||p) ==TRUE) res [j*COL+i]=addpix;
else res[j*COL+i]=delpix;
}}
for(j=StrRow; j<EndRow; j++){
for(i=StrCol; i<EndCol; i++){
if (res[j*COL+i]==addpix) frm1[j*COL+i]=255;
if (res[j*COL+i]==delpix) frm1[j*COL+i]=0;
}}

/*mask6*/
for(j=StrRow; j<EndRow; j++){
for(i=StrCol; i<EndCol; i++){
	if(frm1[(j-1)*COL+(i-1)]>0) a=1; else a=0;
	if(frm1[(j-1)*COL+i]>0 ) b=1; else b=0;
	if(frm1[(j-1)*COL+(i+1)]>0)c=1;else c=0;
	if (frm1[j*COL+(i-1)]>0) d=1; else d=0;
	if (frm1[j*COL+i]>0)p=1;else p=0;
	if (frm1[j*COL+(i+1)]>0) e=1;else e=0;
	if (frm1[(j+1)*COL+(i-1)]>0) f=1;else f=0;
	if (frm1[(j+1)*COL+i]>0) g=1;else g=0;
	if (frm1[(j+1)*COL+(i+1)]>0) h=1;else h=0;
if ((!p&&(b&&c&&e)&&!(a||d||f||g||h)||p) ==TRUE) res [j*COL+i]=addpix;
else res[j*COL+i]=delpix;
}}

/*final image*/
for(j=StrRow; j<EndRow; j++){
for(i=StrCol; i<EndCol; i++){
if (res[j*COL+i]==addpix) frm1[j*COL+i]=255;
if (res[j*COL+i]==delpix) frm1[j*COL+i]=0;
}}
}



/******************************************************/
/*                                                    */
/* This function performs two pass                    */
/* Connected Component labeling algorithm,	    	  */
/* evaluates the results and eliminates small blobs	  */
/* which are smaller than threshold value set in      */
/* header file "traffic.h"                            */       
/* 28-Jan-2000                                        */
/* StrRow: starting row                               */
/* StrCol: starting column                            */
/* EndRow: End row                                    */
/* EndCol: End column                                 */
/* *frm1 : pointer to target image array              */
/* *result : pointer to result labeled image array    */
/*                                                    */
/******************************************************/                
int LabelObjects (int StrRow, int StrCol, int EndRow, int EndCol,int COL, BYTE *frm1, int *res)
{
int i,j,CHANGE; 
int min;
int number=1; 
#define FALSE 0
#define TRUE 1

for (i=StrRow;i<EndRow;i++){for (j=StrCol;j<EndCol;j++){res[i*COL+j]=0;}}

//assign labels 
for (i=StrRow;i<EndRow;i++){
for (j=StrCol;j<EndCol;j++){
if (frm1[i*COL+j]==255){res[i*COL+j]=number;number++;}}}

do {
//do forward pass
CHANGE=FALSE;
for (i=1;i<EndRow-1;i++){for (j=1;j<EndCol-1;j++){
if (res[i*COL+j] >0){min=res[i*COL+j];
if (res[(i-1)*COL+(j-1)] >0){if (res[(i-1)*COL+j-1]<min) min=res[(i-1)*COL+j-1];}	
if (res[(i-1)*COL+j] >0){if (res[(i-1)*COL+j]<min) min=res[(i-1)*COL+j];}	
if (res[(i-1)*COL+(j+1)] >0){if (res[(i-1)*COL+j+1]<min) min=res[(i-1)*COL+j+1];}
if (res[i*COL+(j-1)] >0){if (res[i*COL+j-1]<min) min=res[i*COL+j-1];}
if (res[i*COL+(j+1)] >0){if (res[i*COL+j+1]<min) min=res[i*COL+j+1];}			
if (min < res[i*COL+j]){res[i*COL+j]=min;CHANGE=TRUE;}
}}}
//do reverse pass
for (i=EndRow-1;i>0;i--){for (j=EndCol-1;j>0;j--){ 
if (res[i*COL+j]>0){min=res[i*COL+j];
if (res[(i+1)*COL+j+1]>0){if(res[(i+1)*COL+j+1]<min) min=res[(i+1)*COL+j+1];}	
if (res[(i+1)*COL+j]>0){if(res[(i+1)*COL+j]<min) min=res[(i+1)*COL+j];}	
if (res[(i+1)*COL+j-1]>0){if(res[(i+1)*COL+j-1]<min) min=res[(i+1)*COL+j-1];}
if (res[i*COL+j-1]>0){if(res[i*COL+j-1]<min) min=res[i*COL+j-1];}
if (res[i*COL+j+1]>0){if(res[i*COL+j+1]<min) min=res[i*COL+j+1];}			
if (min < res[i*COL+j]) {res[i*COL+j]=min;CHANGE=TRUE;}
}}}
}while (CHANGE !=FALSE);

return(number+1);
}



int Histogram (int StrRow, int StrCol, int EndRow, int EndCol, int COL, long int minbin,long int maxbin,long int *histogram, int *res)
{
int i,j; 
int count=0,temp; 

for (i=minbin;i<maxbin;i++){histogram[i]=0;}
//get histogram of labels
for (i=StrRow;i<EndRow;i++){
for (j=StrCol;j<EndCol;j++){
	temp=res[i*COL+j];histogram[temp]++;}}

for (i=minbin;i<maxbin;i++){if (histogram[i]>0)count++;}
return (count);
}



int TestObjects(int StrRow, int StrCol, int EndRow, int EndCol, int COL, int *res, long int minbin,long int maxbin, long int *histogram, int obj, BLOBS *cand)
{

int i,j,k; 
int count=0;
int xmin;
int ymin;
int xmax;
int ymax;
float sumRow, sumCol;
float area;
//eliminate small bins which can not be objects.
//treshold set as 5000 pix in header file

for (k=minbin;k<maxbin;k++){
	if (histogram[k]>PixThreshold){
							xmin=EndCol;
							ymin=EndRow;
							xmax=StrCol;
							ymax=StrRow;
							cand[count].area=histogram[k];
							cand[count].no=count;
							cand[count].label=k;
							sumRow=0; sumCol=0; area=1;
							for (i=StrRow;i<EndRow;i++){
							for (j=StrCol;j<EndCol;j++){
							if (res[i*COL+j]==k)
							{
							 if (i<ymin)ymin=i;
							 if (i>ymax)ymax=i;
							 if (j<xmin)xmin=j;
							 if (j>xmax)xmax=j;	
							 sumRow+=i;sumCol+=j;area++;
							}
							}}//for i,j
							cand[count].CornX0=xmin;
							cand[count].CornY0=ymin;
							cand[count].CornX1=xmax;
							cand[count].CornY1=ymax;
							cand[count].CenterY=(int)(sumRow/area);
							cand[count].CenterX=(int)(sumCol/area);

							count++;
							
							}
							}//for k
return(count);
}

void	DrawRectTwoPoint( int X0, int Y0, int X1, int Y1, int COL, int color,int *target)
{
DrawLine (X0,Y0,X1,Y0,COL, color,target);
DrawLine (X0,Y0,X0,Y1,COL, color,target);
DrawLine (X1,Y0,X1,Y1,COL, color,target);
DrawLine (X0,Y1,X1,Y1,COL, color,target);
}

void DrawLine (int x0, int y0, int x1, int y1, int COL, int color, int *target)
{
int i;
int dy, dx, steps;
double xInc, yInc, x, y;

dx = (x1 - x0); dy = (y1 - y0);

if(abs(dx) > abs (dy)){steps = abs(dx);}else{steps = abs(dy);}

xInc = dx/steps; yInc = dy/steps;
x = x0; y = y0;
target[(int)y*COL+(int)x] = color;
for(i=0; i<steps; i++){x = x + xInc;y = y + yInc;
					   target[(int)y*COL+(int)x] = color;
						}
}
              
void conv2d (BYTE *image, int *result, int *mask, int row, int col, int dim_mask)
{
int i,j,ii,jj;
int sum_val=0;
int norm_fact=0;
int border;
/* calculate the normalization factor of the kernel */
border = dim_mask/2;
for (j=0;j<dim_mask;j++)
	{ for (i=0;i<dim_mask;i++)
		{
		norm_fact += abs( *(mask+j*dim_mask+i));
		}
	}
if (norm_fact==0) norm_fact=1; /*if norm_fact is '0' it will cause divide by zero error */
for (j=border;j<row-border;j++)
	{
	for (i=border;i<col-border;i++)
	{
sum_val=0;

for (jj=0;jj<dim_mask;jj++)
		{
		for (ii=0;ii<dim_mask;ii++)
		{
sum_val +=*(mask+jj*dim_mask+ii)* *(image+(j+jj)*col+(i+ii));
		}
		}
	     *(result+j*col+i) = (unsigned char)(sum_val/norm_fact);
	}
}
}


void rotate(int row, int col, int theta, BYTE *in, int *out)
{
int i, j, x,y,xn,yn,xref,yref;
int del_x, del_y;
float rad_ang;

rad_ang=(theta*3.14)/180;
del_x=col/2;
del_y=row/2;


// fill output buffer
for(j=0; j<row; j++){for(i=0; i<col; i++){*(out+j*col+i)=0;}}

// calculate origin


for(j=0; j<row; j++){
for(i=0; i<col; i++){
	x=del_x-i;
	y=del_y-j;
	
	xn=x*cos(rad_ang)-y*sin(rad_ang);
	yn=x*sin(rad_ang)+y*cos(rad_ang);
	xref=xn+del_x;
	yref=yn+del_y;
	if ((yref>=0 && yref<row) && (xref>=0&& xref<col)){*(out+yref*col+xref)=*(in+j*col+i);}
}}

}



//3x3 fast median function
//image: pointer to input image array
//result: pointer to output image array
//row and col: number of row and columns of image
void median3x3(int *image, int *result, int row, int col)
{
register int i,j;
register int column=col;
int col1[3];
int col2[3];
int col3[3];
int final[3];

for (j=1;j<row-1; j++)
	{
	for (i=1; i<col-1; i++)
	{
	col1[0]= *(image+(j-1)*column+(i-1));
	col1[1]= *(image+j*column+(i-1));
	col1[2]= *(image+(j+1)* column +(i-1));
	sort (col1);
	col2[0]= *(image+(j-1)* column +i);
	col2[1]= *(image+j* column +i);
	col2[2]= *(image+(j+1)* column +i);
	sort (col2);
	col3[0]= *(image +(j-1)* column +(i+1));
	col3[1]= *(image +j* column +(i+1));
	col3[2]= *(image +(j+1)* column +(i+1));
	sort (col3);
	if (col1[0] > col2[2])
		final[0]=col1[0];
		else
		final[0]=col2[2];
	if (col2[0] < col3[2])
		final[1]=col2[0];
		else
		final[1]=col3[2];
	final[2]=col2[1];
	sort (final);
	*(result+j*column+i)=final[1];
	}
	}
}

// fast sort of an 1x3 array 
void sort(int *array)
{
int temp[3];
if (array[0] > array[1])
		{
		if (array[1] > array[2])
			{
			temp[2]=array[0];
			temp[1]=array[1];
			temp[0]=array[2];
			}
		else
		    {
			if ( array[0] > array[2])
				{
				temp[2]=array[0];
				temp[1]=array[2];
				temp[0]=array[1];
				}
			 else
			      {
				temp[2]=array[2];
				temp[1]=array[0];
				temp[0]=array[1];
			       }
		      }
		}
else
   {
	 if (array[1]> array[2])
	    {
	       if (array[0]>array[2])
		  {
		  temp[2]=array[1];
		  temp[1]=array[0];
		  temp[0]=array[2];
		  }
	      else
		 {
		 temp[2]=array[1];
		 temp[1]=array[2];
		 temp[0]=array[0];
		 }
	    }
	 else
		 {
		 temp[2]=array[2];
		 temp[1]=array[1];
		 temp[0]=array[0];
		 }
   }
array[0] = temp[0];
array[1] = temp[1];
array[2] = temp[2];

}