struct mds_tag* mds_number_verts_bfs(struct mds_apf* m)
{
struct mds_tag* tag;
mds_id label;
mds_id v;
tag = mds_create_tag(&m->tags, "mds_number", sizeof(mds_id), 1);
label = 0;
v = find_seed(m);
number_connected_verts(&m->mds, v, tag, &label);
for (v = mds_begin(&m->mds, 0); v != MDS_NONE; v = mds_next(&m->mds, v))
number_connected_verts(&m->mds, v, tag, &label);
assert(label == m->mds.n[MDS_VERTEX]);
return tag;
}
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
{
int this_loc, i;
int nx_psi, ny_psi;
int nx_mag, ny_mag;
int nx, ny;
double *psi_uw;
double *psi_w;
double *mag;
double *magth;
double *mask;
double psi_p;
int *visited, *trusted;
int *stack;
int imsize;
int seed_loc;
int nstack;
int nmask;
int nunwrapped;
int m;
double dp;
/* Check for correct number of arguments */
mxAssert(nrhs == 3,
"MEX_Unwrap2D: [psi_uw, mask] = MEX_unwrap2d(psi_w, mag, magthresh)");
mxAssert(nlhs == 2,
"MEX_Unwrap2D: [psi_uw, mask] = MEX_unwrap2d(psi_w, mag, magthresh)");
/* Get matrix dimensions */
nx_psi = mxGetM(PSI_W_MAT);
ny_psi = mxGetN(PSI_W_MAT);
nx_mag = mxGetM(MAG_MAT);
ny_mag = mxGetN(MAG_MAT);
mxAssert((nx_psi == nx_mag) && (ny_psi == ny_mag),
"MEX_Unwrap2D: psi and mag images are different sizes");
nx = nx_psi;
ny = ny_psi;
imsize = nx * ny;
/* Create a real matrix for the return arguments */
PSI_UW_MAT = mxCreateDoubleMatrix(nx, ny, mxREAL);
MASK_MAT = mxCreateDoubleMatrix(nx, ny, mxREAL);
/* Make space for internal matrices and stacks */
trusted = (int *)mxCalloc(imsize, sizeof(int));
visited = (int *)mxCalloc(imsize, sizeof(int));
stack = (int *)mxCalloc(imsize, sizeof(int));
/* Get the data pointers */
psi_w = mxGetPr(PSI_W_MAT);
psi_uw = mxGetPr(PSI_UW_MAT);
mag = mxGetPr(MAG_MAT);
magth = mxGetPr(MAGTH_MAT);
mask = mxGetPr(MASK_MAT);
/************************************************************
* Initialize unwrapped phase matrix, visited map
************************************************************/
nmask = 0;
iloop {
psi_uw[i] = psi_w[i];
trusted[i] = (mag[i] >= *magth);
visited[i] = 0; /* Unvisited*/
mask[i] = trusted[i];
if (mask[i] == 1) nmask++;
}
/************************************************************
* Find the initial seed location
************************************************************/
find_seed(&seed_loc,
mag,
visited,
trusted,
stack,
&nstack,
&nunwrapped,
nx, ny);
/************************************************************
* MAIN REGION GROWING LOOP
************************************************************/
nunwrapped = 0;
while (nunwrapped < nmask) {
while (nstack > 0 && nstack < imsize) {
/* Get location at top of stack */
this_loc = stack[0];
/* Remove this location from the stack */
downstack(&nstack, stack);
psi_p = predict_phase(this_loc,
psi_uw,
visited,
trusted,
nx, ny);
/* Estimate the ambiguity factor for the wrapped phase */
dp = psi_p - psi_w[this_loc];
m = (int)(fabs(dp) / TWO_PI + 0.5);
m = (dp < 0.0) ? -m : m;
/* Unwrap the phase using the ambiguity estimate */
psi_uw[this_loc] = psi_w[this_loc] + m * TWO_PI;
/* Mark this location as unwrapped */
visited[this_loc] = 2;
nunwrapped++;
/* Add this locations neighbours to the end of the stack */
add_neighbours(this_loc,
stack,
visited,
trusted,
&nstack,
nx, ny);
} /* While stack is not empty */
find_seed(&seed_loc,
mag,
visited,
trusted,
stack,
&nstack,
&nunwrapped,
nx, ny);
/************************************************************
* Estimate the phase ambiguity at this seed based on
* previously unwrapped regions
************************************************************/
m = seed_ambiguity(seed_loc,
psi_uw,
visited,
nx, ny);
/* Correct the ambiguity before continuing with the new region growth */
psi_uw[seed_loc] = psi_w[seed_loc] + m * TWO_PI;
} /* While unwrapped pixels still exist in trusted regions */
if (nstack >= imsize)
mexPrintf("Stack grew too large - aborting\n");
/* Clean up */
mxFree(visited);
mxFree(trusted);
mxFree(stack);
}
