smw.tar.gz_Matching_SMW_smw.tar.gz_stereo_stereo matching

/* the functions of this module implements the ssd computation and other tools. * * MVL - A . Fusiello 1997 * */ /********** include files for iterface and private functions ****************/ #include <stdio.h> #include <stdlib.h> #include <math.h> #include <assert.h> #include <nr.h> #include "array2.h" #ifdef EXTERN #undef EXTERN #endif #define EXTERN /************************** public and private headers *************************/ #include "smwp.h" #include "smwpP.h" /******************************************************************************** * * * * * function: smw_stereo (symmetric multi-window stereo) * * inputs: **mat1,**mat2,rows,cols,dxmin,dxmax * * outputs: **d,**R,*frow_p,*fcol_p,*nr_p,*nc_p * * * * Take two images (mat1 and mat2) of dimension rows by cols as inputs * * and produce a disparity map (d) with its uncertainty (R) as output, * * with a region of interest specified by frow_p, fcol_p, nr_p, nc_p. * * dxmin and dxmax bound the disparity search range. * * * * Modified 28/06/97 by A. Fusiello to avoid recomputing SSD for all the windows* * In this version SSD is computed only once with asquare symmetric window * * * ********************************************************************************/ void smw_stereo(u_char **mat1, u_char **mat2, int const rows, int const cols, int const dxmin, int const dxmax, float **d, float **R, int *frow_p, int *fcol_p, int *nr_p, int *nc_p) { int i, j, k, up_col, low_col, frow, fcol, nr, nc, *p1_b, *p2_b, **p1, **p2, *iaux_b, *l_disp, *r_disp, *winner, **disparity[2*NUM_OF_WIN+2]; float *faux_b, *r_resd, *r_sub, *l_resd, **residual[2*NUM_OF_WIN+2], **subpixel[2*NUM_OF_WIN+2]; l_disp =(int *) calloc(cols,sizeof(int) ); r_disp =(int *) calloc(cols,sizeof(int) ); winner =(int *) calloc(cols,sizeof(int) ); r_sub=(float *) calloc(cols,sizeof(float) ); r_resd =(float *) calloc(cols,sizeof(float) ); l_resd =(float *) calloc(cols,sizeof(float) ); p1_b =(int *) calloc(rows*cols,sizeof(int) ); p1 = iarray(p1_b, rows, cols); p2_b =(int *) calloc(rows*cols,sizeof(int) ); p2 = iarray(p2_b, rows, cols); /* array of matrices */ for (k=0; k<2; k++) { iaux_b =(int *) calloc(rows*cols,sizeof(int)); disparity[k] = iarray(iaux_b, rows, cols); } for (k=0; k<2; k++) { faux_b =(float *) calloc(rows*cols,sizeof(float)); residual[k] = farray(faux_b, rows, cols); } for (k=0; k<2; k++) { faux_b =(float *) calloc(rows*cols,sizeof(float)); subpixel[k] = farray(faux_b, rows, cols); } /* lookup tables for fast ssd */ for (j=0; j <cols; j++) for (i = 0; i < rows ; i++) p2[i][j] = ISQR(mat2[i][j]); for (j=0; j <cols; j++) for (i = 0; i < rows; i++) p1[i][j] = ISQR(mat1[i][j]); /* initilizations */ for (j=0; j <cols; j++) for (i = 0; i < rows; i++) { d[i][j]=DCCDISP; R[i][j]=HUGEVAR; } if (dxmax >0 ) { low_col = dxmax; up_col = cols-dxmax; } else { low_col = -dxmin; up_col = cols + dxmin; } frow=WHS; fcol=low_col+WHS; nr=rows-2*WHS; nc=up_col-low_col-2*WHS; /* compute SSD from left to right and from right to left */ fast_ssd(mat1,mat2,p1,p2,rows,cols, dxmin, dxmax,wstable[0],residual[0],disparity[0],subpixel[0]); fast_ssd(mat2,mat1,p2,p1,rows,cols,-dxmax,-dxmin,wstable[0],residual[1],disparity[1],subpixel[1]); for (i=frow; i<frow+nr; i++) { for (j=fcol; j<fcol+nc; j++) { int np,med,ii,jj; static float dis_point[NUM_OF_WIN+1], res_point[NUM_OF_WIN+1], win_point[NUM_OF_WIN+1]; /* RIGHT DISPARITY */ /* compute point disparity */ for (k=0; k<NUM_OF_WIN; k++) { ii = i+ (-wstable[k][0]+wstable[k][1])/2; jj = j+ (-wstable[k][2]+wstable[k][3])/2; dis_point[k+1]=(float)disparity[0][ii][jj]; res_point[k+1]=(float)residual[0][ii][jj]; win_point[k+1]=(float)k; } sort3(NUM_OF_WIN,res_point,dis_point,win_point); /* the minimun residual could be achieved by more then one windows, giving different disparities; in that case we use a majotity voting to assign disparity */ np=1; while(res_point[1]==res_point[np+1] && np < NUM_OF_WIN) np++; /* sort first np elements by disparity value */ sort3(np,dis_point,res_point,win_point); /* take the median */ med = (np+1)/2; assert(med >=1 && med <=np); /* ASSERT */ r_disp[j]= (int)dis_point[med]; r_resd[j]= res_point[med]; /* assign subpixel adjustment */ winner[j]=(int)win_point[med]; ii = i+ (-wstable[winner[j]][0]+wstable[winner[j]][1])/2; jj = j+ (-wstable[winner[j]][2]+wstable[winner[j]][3])/2; r_sub[j]= subpixel[0][ii][jj]; /* compute variance */ { float ave; /* unused variable */ variance(dis_point,NUM_OF_WIN,&ave,&R[i][j]); R[i][j]+= ACCURACY; } /* LEFT DISPARITY */ /* compute point disparity */ for (k=0; k<NUM_OF_WIN; k++) { ii = i+ (-wstable[k][0]+wstable[k][1])/2; jj = j+ (-wstable[k][2]+wstable[k][3])/2; dis_point[k+1]=(float) disparity[1][ii][jj]; res_point[k+1]=(float)residual[1][ii][jj]; win_point[k+1]=(float)k; } sort3(NUM_OF_WIN,res_point,dis_point,win_point); /* the minimun residual could be achieved by more then one windows, giving different disparities; in that case we use a majotity voting to assign disparity */ np=1; while(res_point[1]==res_point[np+1] && np < NUM_OF_WIN) np++; /* sort first np elements by disparity value */ sort3(np,dis_point,res_point,win_point); /* take the median */ med = (np+1)/2; assert(med >=1 && med <=np); /* ASSERT */ l_disp[j]=(int)dis_point[med]; l_resd[j]= res_point[med]; } /* next j */ /* HERE IS THE CORE OF THE ALGORITHM */ for (j=fcol; j<fcol+nc; j++) { int jj=(int)j+r_disp[j]; /* left image abscissa */ assert (jj >= 0 && jj< cols); /* ASSERT */ if (jj >= fcol+nc) l_disp[jj] =- r_disp[j]; /* check left-right consistency */ if (r_disp[j]==-l_disp[jj]) { /* OK */ d[i][j]=(float)r_disp[j] +r_sub[j]; assert( fabs(r_sub[j]) < 0.5); /* ASSERT */ } else { /* occlusion */ d[i][j]=(float)dxmin; /* it's a bad guess */ R[i][j]=HUGEVAR; } } /* next j */ /* assigning a disparity to occlusions */ for (j=fcol+nc-2; j>=fcol; j--) { if (d[i][j]==dxmin && R[i][j]==HUGEVAR) d[i][j]=d[i][j+1]; } /* next j */ } /* next i */ /* ... finally, clip disparity within boundaries */ for (i=frow; i<frow+nr; i++) for (j=fcol; j<fcol+nc; j++) { if (d[i][j] > dxmax) { d[i][j] = dxmax; R[i][j] = HUGEVAR; } if (d[i][j] < dxmin) { d[i][j] = dxmin; R[i][j] = HUGEVAR; } assert(R[i][j] > 0); /* ASSERT */ } /* next j */ /* next i */ /* set reference parameters (ROI) */ *frow_p = frow; *fcol_p = fcol; *nr_p = nr; *nc_p = nc; /* clear memory */ free(p1_b); free(p1); free(p2_b); free(p2); for (k=0; k<2; k++) { free (disparity[k][0]); free (disparity[k]); } for (k=0; k<2; k++) { free(residual[k][0]); free(residual[k]); } for (k=0; k<2; k++) { free(subpixel[k][0]); free(subpixel[k]); } } /*************************************************************************************** * * * * * function: fast_ssd * *