POTS.tar.gz.gz_VHDL camera_irobot _object tracking vhdl_robot_vi

/******************** img_tracking.c ***************************************** /* Author: Baolin Shao * Date: 2009 04 10 * Description: This file has a very simple object tracking algorithm. * The algoirthm "tracks" an object based on the object's * position. Given the object's previuos centroid, this * algorithm calcutes the object's current centroid. * This algorithm works based on the following assumption: * 1. The object's initial position is in the center of image * 2. The object does not move dramatically between two * succesive images * 3. The object's color is reasonably different from its * background ***************************************************************************/ #include "stdlib.h" #include "stdio.h" #include "img_tracking.h" inline void comp(int *pt_r,int *pt_c) { if(*pt_r<0) *pt_r = 0; if(*pt_r>ROW) *pt_r = ROW-1; if(*pt_c<0) *pt_r = 0; if(*pt_c>COL) *pt_r = COL-1; } void get_obj_def(short *buf, int center_row,int center_col, unsigned char *val_r, unsigned char *val_g, unsigned char *val_b) { int c_offset; int i; c_offset = OFFSET(center_row,center_col); short v = IORD_SDRAM(buf+c_offset); *val_r = GET_R(v); *val_g = GET_G(v); *val_b = GET_B(v); } void get_obj_val_rgb(short *buf, int center_row,int center_col, unsigned char *val_r, unsigned char *val_g, unsigned char *val_b) { unsigned short val =0; int i; int sumr,sumg,sumb; sumr=sumg=sumb=0; *val_r=0; *val_g=0; *val_b=0; int c_offset; /******************************************/ /* b b 1 b b */ /* b 2 x 3 b */ /* 4 x c x 5 */ /* b 6 x 7 b */ /* b b 8 b b */ /******************************************/ int p_r[8]; int p_c[8]; p_r[0] = center_row-2; p_c[0] = center_col; p_r[1] = center_row-1; p_c[1] = center_col-1; p_r[2] = center_row-1; p_c[2] = center_col+1; p_r[3] = center_row; p_c[3] = center_col-2; p_r[4] = center_row; p_c[4] = center_col+2; p_r[5] = center_row+1; p_c[5] = center_col-1; p_r[6] = center_row+1; p_c[6] = center_col+1; p_r[7] = center_row+2; p_c[7] = center_col; for(i=0;i<8;i++) { comp(p_r+i,p_c+i); c_offset = OFFSET(p_r[i],p_c[i]); val=IORD_SDRAM(buf+c_offset); sumr+=GET_R(val); sumg+=GET_G(val); sumb+=GET_B(val); } *val_r = (unsigned char)(sumr>>3); *val_g = (unsigned char)(sumg>>3); *val_b = (unsigned char)(sumb>>3); int of = OFFSET(center_row,center_col); unsigned short cv = IORD_SDRAM(buf+of); unsigned char cvr = GET_R(cv); unsigned char cvg = GET_G(cv); unsigned char cvb = GET_B(cv); } void get_obj_val(short *buf, int center_row,int center_col, unsigned char *cval) { unsigned short val; unsigned char v_r,v_g,v_b; val=0; v_r=v_g=v_b=0; int i,j; int c_offset; int left,right,top,bottom; left = center_col - 2; right = center_col + 1; top = center_row - 2; bottom = center_row + 1; if(left < 0) left =0; if(right > COL) right = COL; if(top < 0) top =0; if(bottom > ROW) bottom = ROW; for(i=top;i<=bottom;i++) for (j=left;j<=right;j++) { c_offset = OFFSET(i,j); val=IORD_SDRAM(buf+c_offset); v_r=GET_R(val); v_g=GET_G(val); v_b=GET_B(val); val = CONVERT(v_r,v_g,v_b); *cval = *cval + val; } *cval= (*cval)>>4; } static int closeto(int val, int target) { if (val < target + PIC_THRESHOLD && val > target - PIC_THRESHOLD) return 1; return 0; } /******************************* track_obj ********************************** Function: track_obj Parameter: input: short * buf:image data, every pixel constitutes two bytes (1 short). int center_row, center_col: previous centroid of object. output: int *new_center_row, +*new_center_col:newly calculated centroid ****************************************************************************/ void track_obj ( short *buf, int center_row, int center_col, int *new_center_row, int *new_center_col) { int left=COL-1; int right=0; int top=ROW-1; int bottom=16; unsigned short center_val=0; unsigned char center_val_r=0; unsigned char center_val_g=0; unsigned char center_val_b=0; if(center_row==-1 && center_col==-1) { center_row=ROW>>1; center_col=COL>>1; } center_val_r=220; center_val_g=0; center_val_b=220; int i,j,k1,k2; i=j=k1=k2=0; for(i=0;i<ROW;i=i+BLOCK_SIZE) { for(j=0;j<COL;j=j+BLOCK_SIZE) { unsigned short sum_r,sum_g,sum_b; sum_r=sum_g=sum_b=0; for(k1=i;k1<i+BLOCK_SIZE;k1++) { for(k2=j;k2<j+BLOCK_SIZE;k2++) { int offset=OFFSET(k1,k2); unsigned short val=IORD_SDRAM(buf+offset); unsigned char val_r=0; unsigned char val_g=0; unsigned char val_b=0; val_r=GET_R(val); val_g=GET_G(val); val_b=GET_B(val); if(val_r>center_val_r-PIC_THRESHOLD && val_r<center_val_r+PIC_THRESHOLD) sum_r++; } } if( closeto(sum_r, center_val_r) && closeto(sum_b, center_val_b)) { if(i<top) { top=i; } if(i>bottom) { if(i+BLOCK_SIZE>ROW) bottom = ROW-1; else bottom=i+BLOCK_SIZE-1; } if(j<left) { left = j; } if(j>right) { if(j+BLOCK_SIZE>COL) right = COL-1; else right = j+BLOCK_SIZE-1; } } } } *new_center_row = (top+bottom)>>1; *new_center_col = (left+right)>>1; } void track_obj2 ( short *buf, int center_row, int center_col, int *new_center_row, int *new_center_col) { int left=COL-1; int right=0; int top=ROW-1; int bottom=16; unsigned char center_val_r=0; unsigned char center_val_g=0; unsigned char center_val_b=0; unsigned char cval=0; if(center_row==-1 && center_col==-1) { center_row=ROW>>1; center_col=COL>>1; } get_obj_def(buf,center_row,center_col, &center_val_r,&center_val_g,&center_val_b); cval = CONVERT(center_val_r,center_val_g,center_val_b); int i,j,k1,k2; i=j=k1=k2=0; int cnt=0; for(i=16;i<ROW;i=i+BLOCK_SIZE) { for(j=0;j<COL;j=j+BLOCK_SIZE) { unsigned short sum=0; for(k1=i;k1<i+BLOCK_SIZE;k1++) { for(k2=j;k2<j+BLOCK_SIZE;k2++) { int offset=OFFSET(k1,k2); unsigned short val=IORD_SDRAM(buf+offset); unsigned char val_r=0; unsigned char val_g=0; unsigned char val_b=0; unsigned char v=0; val_r=GET_R(val); val_g=GET_G(val); val_b=GET_B(val); v = CONVERT(val_r,val_g,val_b); if(v>200 && v<255) sum++; } } if( sum>BLOCK_THRESHOLD ) { cnt++; if(i<top) { top=i; } if(i>bottom) { if(i+BLOCK_SIZE>ROW) bottom = ROW-1; else bottom=i+BLOCK_SIZE-1; } if(j<left) { left = j;