#define _GNU_SOURCE #include #include #include #include #include #include #define get_sample float_image_get_sample #define set_sample float_image_set_sample #define ISFULL FALSE void weighted_average ( double w0, double v0, double w1, double v1, double vardelta, double *v, double *w ) { if ((w0 == 0) && (w1 == 0)){ (*v) = (v0+v1)/2; (*w) = 0; return; } if ((w0 == INF) && (w1 == INF)){ (*v) = (v0+v1)/2; (*w) = INF; return; } if (vardelta == INF){ (*v) = (v0+v1)/2; (*w) = 4/(1/w0 + 1/w1); return; } if ((w0 == 0) || (w1 == INF)){ (*v) = v1; (*w) = w1; return; } if ((w1 == 0) || (w0 == INF)){ (*v) = v0; (*w) = w0; return; } if (vardelta == 0){ (*v) = (w0*v0+w1*v1)/(w0+w1); (*w) = w0 + w1; return; } // General case - {w0,w1} finite and nonzero, {vardelta} finite: double lambda = (w1*vardelta + 1)/(2*w1*vardelta + w1/w0 + 1); assert((lambda >= 0) && (lambda <= 1)); double adbmal = 1 - lambda; // fprintf(stderr,"lambda interno: % 9.6lf\n",lambda); (*v) = lambda*v0 + adbmal*v1; double fator = (lambda - adbmal)/2; double var_err = fator*fator*vardelta + lambda*lambda/w0 + adbmal*adbmal/w1; (*w) = 1/var_err; } void pst_interpolate_two_samples ( float_image_t* I, float_image_t* W, int c, int x0, int y0, int x1, int y1, double *v, double* w ) { if( (I == NULL) && (W == NULL) ){ *v = 0; *w = 1; return; } int NX = (I == NULL ? W->sz[1] : I->sz[1]); int NY = (I == NULL ? W->sz[2] : I->sz[2]); if( (I != NULL) && (W != NULL) ){ assert( (I->sz[1] == W->sz[1]) && (I->sz[2] == W->sz[2]));} double v0 = ( (x0 < 0) || (y0 < 0) || (x0 >= NX) || (y0 >= NY) ? 0 : (I == NULL ? 0 : float_image_get_sample(I,c,x0,y0))); double v1 = ( (x1 < 0) || (y1 < 0) || (x1 >= NX) || (y1 >= NY) ? 0 : (I == NULL ? 0 : float_image_get_sample(I,c,x1,y1))); double w0 = ( (x0 < 0) || (y0 < 0) || (x0 >= NX) || (y0 >= NY) ? 0 : (W == NULL ? 1 : float_image_get_sample(W,0,x0,y0))); double w1 = ( (x1 < 0) || (y1 < 0) || (x1 >= NX) || (y1 >= NY) ? 0 : (W == NULL ? 1 : float_image_get_sample(W,0,x1,y1))); /* First we get the interpolation of one diagonal*/ *v = (v0+v1)/2; *w = 4.0/(1/w0 + 1/w1); } void pst_interpolate_four_samples ( float_image_t* I, float_image_t* W, int c, int xm, int ym, int x0, int y0, int x1, int y1, int xp, int yp, double *v, double* w ) { if( (I == NULL) && (W == NULL) ){ *v = 0; *w = 1; return; } int NX = (I == NULL ? W->sz[1] : I->sz[1]); int NY = (I == NULL ? W->sz[2] : I->sz[2]); if( (I != NULL) && (W != NULL) ){ assert( (I->sz[1] == W->sz[1]) && (I->sz[2] == W->sz[2]));} double v0 = ( (x0 < 0) || (y0 < 0) || (x0 >= NX) || (y0 >= NY) ? 0 : (I == NULL ? 0 : float_image_get_sample(I,c,x0,y0))); double v1 = ( (x1 < 0) || (y1 < 0) || (x1 >= NX) || (y1 >= NY) ? 0 : (I == NULL ? 0 : float_image_get_sample(I,c,x1,y1))); double w0 = ( (x0 < 0) || (y0 < 0) || (x0 >= NX) || (y0 >= NY) ? 0 : (W == NULL ? 1 : float_image_get_sample(W,0,x0,y0))); double w1 = ( (x1 < 0) || (y1 < 0) || (x1 >= NX) || (y1 >= NY) ? 0 : (W == NULL ? 1 : float_image_get_sample(W,0,x1,y1))); double vm = ( (xm < 0) || (ym < 0) || (xm >= NX) || (ym >= NY) ? 0 : (I == NULL ? 0 : float_image_get_sample(I,c,xm,ym))); double vp = ( (xp < 0) || (yp < 0) || (xp >= NX) || (yp >= NY) ? 0 : (I == NULL ? 0 : float_image_get_sample(I,c,xp,yp))); double wm = ( (xm < 0) || (ym < 0) || (xm >= NX) || (ym >= NY) ? 0 : (W == NULL ? 1 : float_image_get_sample(W,0,xm,ym))); double wp = ( (xp < 0) || (yp < 0) || (xp >= NX) || (yp >= NY) ? 0 : (W == NULL ? 1 : float_image_get_sample(W,0,xp,yp))); /* First we get the interpolation of one diagonal*/ pst_interpolate_four_points(vm,wm,v0,w0,v1,w1,vp,wp,v,w); } void pst_interpolate_four_points ( double vm, double wm, double v0, double w0, double v1, double w1, double vp, double wp, double *v, double *w ) { double dist_factor = 0.25; double va = (3*v0 - vm)/2.0; double wa = dist_factor*4.0/(9.0/w0 + (1.0/(w0*wm))); double vb = (v0+v1)/2.0; double wb = 4.0/(1.0/w0 + 1.0/w1); double vc = (3*v1 - vp)/2.0; double wc = dist_factor*4.0/(9.0/w1 + (1.0/(w1*wp))); if( (wa == 0) && (wb == 0) && (wc == 0) ){ *w = 0; *v = vb; }else{ *w = wa + wb + wc; *v = (wa*va + wb*vb + wc*vc)/(wa + wb + wc); } } void pst_slope_and_weight_map_shrink ( float_image_t *IG, float_image_t *IW, float_image_t **SG, float_image_t **SW ) { int NC = IG->sz[0]; int NXI = IG->sz[1]; int NXJ = (NXI+1)/2; int NYI = IG->sz[2]; int NYJ = (NYI+1)/2; float_image_t *JG = float_image_new(NC, NXJ, NYJ); float_image_t *OW = NULL; if (IW != NULL) { assert(IW->sz[0] == 1); assert(IW->sz[1] == NXI); assert(IW->sz[2] == NYI); OW = float_image_new(1, NXJ, NYJ); } int jx, jy; for (jy = 0; jy < NYJ; jy++) { for (jx = 0; jx < NXJ; jx++) { int ix = 2*jx, iy = 2*jy; double wmin = INF; int c; for( c = 0; c < 2; c++) { double da,wa; pst_interpolate_two_samples(IG,IW,c,ix,iy,ix+1,iy+1,&da,&wa); double db,wb; pst_interpolate_two_samples(IG,IW,c,ix,iy+1,ix+1,iy,&db,&wb); double w = 4/(1/wa+1/wb); double d = ( (wa+wb) > 0 ? (wa*da + wb*db)/(wa + wb) : da+db/2.0); float_image_set_sample(JG, c, jx, jy, d); if(w < wmin) wmin = w; } if (OW != NULL) { float_image_set_sample(OW, 0, jx, jy, wmin);} } } *SG = JG; *SW = OW; } #define pst_slope_map_size_cutoff 1 /* Do not recurse on images smaller than this. */ void pst_slope_map_to_depth_map_recursive ( float_image_t *IG, float_image_t *IW, int level, bool_t newStyle, long int maxIter, long int minMapSize, double convTol, float_image_t **OZ, float_image_t **OW, bool_t verbose, long int reportIter, pst_slope_map_report_proc_t *reportData, pst_imgsys_report_proc_t *reportSys, pst_height_map_report_proc_t *reportHeights ) { /* Get image size and check compatibility: */ demand(IG->sz[0] == 2, "wrong {IG} channels"); int NX_G = IG->sz[1]; int NY_G = IG->sz[2]; if (IW != NULL) { demand(IW->sz[0] == 1, "wrong {IW} channels"); demand(IW->sz[1] == NX_G, "wrong {IW} cols"); demand(IW->sz[2] == NY_G, "wrong {IW} rows"); } /* Allocate output images: */ int NX_Z = NX_G + 1; int NY_Z = NY_G + 1; (*OZ) = float_image_new(1, NX_Z, NY_Z); (*OW) = float_image_new(1, NX_Z, NY_Z); int indent = 2*level+2; /* Indentation for messages. */ if (verbose) { fprintf(stderr, "%*sEntering level %d with G size %dx%d ...\n", indent, "", level, NX_G, NY_G); } if (reportData != NULL) { reportData(level, IG, IW); } /* Decide whether to use multi-scale: */ bool_t small = ((NX_G < pst_slope_map_size_cutoff) && (NY_G < pst_slope_map_size_cutoff)); bool_t atomic = ((NX_G == 1) || (NY_G == 1)); bool_t trivial = ((NX_G == 1) && (NY_G == 1)); bool_t direct = (newStyle ? trivial : (small || atomic)); // int minMapSize = 16; if( (NX_G <= minMapSize) || (NY_G <= minMapSize) ){ direct = TRUE; } /* Get the initial guess {OZ} for the heights: */ if (direct) { /* Initialize {OZ} with zeros: */ if (newStyle) { /* Solution for the 1x1 system: */ /* Get slopes: */ float dZdX = get_sample(IG, 0, 0, 0); float dZdY = get_sample(IG, 1, 0, 0); /* Assume an affine function that is 0 at center, with the given gradient: */ double z10 = 0.5*(dZdX - dZdY); double z11 = 0.5*(dZdX + dZdY); set_sample(*OZ, 0, 0, 0, -z11); set_sample(*OZ, 0, 0, 1, -z10); set_sample(*OZ, 0, 1, 0, +z10); set_sample(*OZ, 0, 1, 1, +z11); } else { /* Set all to zero: */ int x, y; for(y = 0; y < NY_Z; y++) for(x = 0; x < NX_Z; x++) { set_sample(*OZ, 0, x, y, 0.0); } } } else { /* Initialize {OZ} with the solution to a coarser problem. */ /* Shrink slope maps and weight map by half: */ if (verbose) { fprintf(stderr, "%*sShrinking slope and weight maps ...\n", indent, ""); } float_image_t *SIG; float_image_t *SIW; pst_slope_and_weight_map_shrink(IG,IW,&SIG,&SIW); /* Compute the half-scale height map: */ float_image_t *SOZ, *SOW; pst_slope_map_to_depth_map_recursive ( SIG, SIW, level+1, newStyle, 2*maxIter,minMapSize, convTol/2.0, &SOZ, &SOW, verbose, reportIter, reportData, reportSys, reportHeights ); /* Expand the computed height map to double size: */ if (verbose) { fprintf(stderr, "%*sExpanding height map to %dx%d ...\n", indent, "", NX_Z, NY_Z); } int expOrder = 2; /* Bilinear for now. !!! Generalize? !!! */ *OZ = pst_height_map_expand(SOZ, SOW, NX_Z, NY_Z, expOrder); /* Free the working storage: */ float_image_free(SIG); if (SIW != NULL) { float_image_free(SIW); } float_image_free(SOZ); if (SOW != NULL) { float_image_free(SOW); } } bool_t solve_sys = (! (newStyle && trivial)); if (solve_sys) { /* Build the linear system: */ long int NP_Z = NX_Z*NY_Z; if (verbose) { fprintf(stderr, "%*sBuilding linear system for %ld pixels ...\n", indent, "", NP_Z); } bool_t full = ISFULL; /* Should be parameter. FALSE means exclude indeterminate pixels. */ imgsys_t* S = pst_slope_map_build_integration_system(IG, IW, full, *OW); if (verbose) { fprintf(stderr, "%*sSystem has %ld equations and %ld unknowns\n", indent, "", S->N, S->N); } if (reportSys != NULL) { reportSys(level, S); } /* Solve the system for the corner heights: */ if (verbose) { fprintf(stderr, "%*sSolving the system ...\n", indent, ""); } int para = 0; /* Should be parameter. 1 means parallel execution, 0 sequential. */ int szero = 1; /* Should be parameter. 1 means adjust sum to zero, 0 let it float. */ pst_slope_map_solve_system(S, *OZ, *OW, maxIter, convTol, para, szero, verbose, level, reportIter, reportHeights); pst_imgsys_free(S); } else { /* The initial guess is the solution: */ if (reportSys != NULL) { reportSys(level, NULL); } if (reportHeights != NULL) { reportHeights(level, 0, 0.0, TRUE, *OZ, *OW); } } if (verbose) { fprintf ( stderr, "%*sLeaving level %d (OZ = %lldx%lld) ...\n", indent, "", level, (*OZ)->sz[1], (*OZ)->sz[2] ); } } void pst_compute_edge_slope_and_weight ( float_image_t *G, float_image_t *GW, int axis, int x,int y, double *d, double *w ) { if (axis == 0) { /* X derivative */ pst_interpolate_four_samples(G,GW,0,x,y-2,x,y-1,x,y,x,y+1,d,w); } else if (axis == 1) { /* Y derivative */ pst_interpolate_four_samples(G,GW,1,x-2,y,x-1,y,x,y,x+1,y,d,w); } else { demand(FALSE, "invalid axis"); } return; } imgsys_t* pst_slope_map_build_integration_system ( float_image_t *G, float_image_t *GW, bool_t full, float_image_t *ZW ) { /* Get/check the sizes of the slope maps: */ int NC = G->sz[0]; assert(NC == 2); int NX_G = G->sz[1]; int NY_G = G->sz[2]; if (GW != NULL) { assert(GW->sz[0] == 1); assert(GW->sz[1] == NX_G); assert(GW->sz[2] == NY_G); } /* Check the size of the system: */ int NX_Z = NX_G+1; int NY_Z = NY_G+1; int NXY_Z = NX_Z*NY_Z; /* Number of unknowns (heights). */ if (ZW != NULL) { demand(ZW->sz[0] == 1, "wrong {ZW} channels"); demand(ZW->sz[1] == NX_Z, "wrong {ZW} cols"); demand(ZW->sz[2] == NY_Z, "wrong {ZW} rows"); } /* Conceptually, we define two quadratic mismatch terms {qx[x,y](h)} and {qy[x,y](h)} for each edge in the pixel grid. The term {qx[x,y](h)} is the square of the difference between the mean slope along the edge from {(x,y)} to {(x+1,y)}, as estimated from the slope map {G}, and the mean slope computed by numerical difference of the (unknown) heights at the two endpoints of that edge. This terms exists for all {x} in {0..NX_G-1} and all {y} in {0..NY_G}. { qx[x,y](h) = (dx[x,y] - (- h[k00] + h[k10]))^2 } where {h[k00]} and {h[k10]} are the {h} variables corresponding to heights {z[x,y]} and {z[x+1,y]}, respectively; and {dx} is the interpolated slope { dx[x,y] = (wpm*dxpm + wpp*dxpp)/wx[x,y] } where {wpm == IW[0,x,y-1]}, {wpp == IW[0,x,y]}, {wx[x,y] == wpm+wpp}, {dxpm == G[0,x,y-1]}, and {dxpp == G[0,x,y]}. The term {qy[x,y](h)} is similarly defined for edges parallel to the Y axis. It exists for all {x} in {0..NX_G} and all {y} in {0..NY_G-1}. { qy[x,y](h) = (dy[x,y] - (- h[k00] + h[k01]))^2 } { dy[x,y] = (wmp*dxmp + wpp*dxpp)/wy[x,y] } where {wmp == IW[0,x,y-1]}, {wy[x,y] == wmp+wpp}, {dymp == G[1,x-1,y]}, and {dypp == G[1,x,y]}. We want to minimize the sum {Q(h)} of all terms {qx[x,y](h)} and {qy[x,y](h)}, each weighted by the corresponding edge weight {wx[x,y]} or {wy[x,y]}. Note that if the slope map {G} is zero, we can anihilate all those terms (and therefore Q) by setting all heights equal to 1. Therefore, if the system is to be solved iteratively, one must take care to exclude this solution. Minimizing {Q} is equivalent to finding the {h} vector that anihilates the gradient {G} of {Q}. The gradient of {Q} is an affine function of {h}, namely {G(h) = A h - b}. Each term {wx[x,y]*qx[x,y](h)} contributes to two components of the gradient, namely those with indices {k00,k10}. Similarly, each term {wy[x,y]*qy[x,y]} contributes to two components, with indices {k00,k01}. Thus, to build the system, we add those contributions to the appropriate elements of {A} and {b}. */ /* Make sure that we can build the system: */ assert(MAXCOEFS >= 5); /* Gather the valid equations {eq[0..N-1]} of the system, and fill the {ix[0..NXY_Z-1]} table: */ long int *ix = (long int *)notnull(malloc(NXY_Z*sizeof(long int)), "no mem"); imgsys_equation_t *eq = (imgsys_equation_t*)notnull(malloc(NXY_Z*sizeof(imgsys_equation_t)), "no mem"); long int N = 0; long int xy; for (xy = 0; xy < NXY_Z; xy++) { /* Get the indices of pixel number {k}: */ int x = xy % NX_Z; int y = xy / NX_Z; /* Get the slopes along edges incident to {(x,y)}, and their weights */ double wxm, wxp, wym, wyp; /* Edge weights. */ double dxm, dxp, dym, dyp; /* Edge slopes. */ pst_compute_edge_slope_and_weight(G,GW,0,x+0,y+0,&dxp,&wxp); pst_compute_edge_slope_and_weight(G,GW,1,x+0,y+0,&dyp,&wyp); pst_compute_edge_slope_and_weight(G,GW,0,x-1,y+0,&dxm,&wxm); pst_compute_edge_slope_and_weight(G,GW,1,x+0,y-1,&dym,&wym); assert((! isnan(wxm)) && (wxm >= 0)); assert((! isnan(wxp)) && (wxp >= 0)); assert((! isnan(wym)) && (wym >= 0)); assert((! isnan(wyp)) && (wyp >= 0)); double dxmm = 0.0, dxmp = 0.0, dxpm = 0.0, dxpp = 0.0; double dymm = 0.0, dymp = 0.0, dypm = 0.0, dypp = 0.0; double wmm = 0.0, wmp = 0.0, wpm = 0.0, wpp = 0.0; if ((x > 0) && (y > 0)) { dxmm = get_sample(G, 0, x-1, y-1); dymm = get_sample(G, 1, x-1, y-1); wmm = (GW == NULL ? 1.0 : get_sample(GW, 0, x-1, y-1)); assert(wmm >= 0.0); } if ((x > 0) && (y < NY_G)) { dxmp = get_sample(G, 0, x-1, y+0); dymp = get_sample(G, 1, x-1, y+0); wmp = (GW == NULL ? 1.0 : get_sample(GW, 0, x-1, y+0)); assert(wmp >= 0.0); } if ((x < NX_G) && (y > 0)) { dxpm = get_sample(G, 0, x+0, y-1); dypm = get_sample(G, 1, x+0, y-1); wpm = (GW == NULL ? 1.0 : get_sample(GW, 0, x+0, y-1)); assert(wpm >= 0.0); } if ((x < NX_G) && (y < NY_G)) { dxpp = get_sample(G, 0, x+0, y+0); dypp = get_sample(G, 1, x+0, y+0); wpp = (GW == NULL ? 1.0 : get_sample(GW, 0, x+0, y+0)); assert(wpp >= 0.0); } bool_t axial_xm = FALSE, axial_xp = FALSE, axial_ym = FALSE, axial_yp = FALSE; bool_t diagn_mm = FALSE, diagn_mp = FALSE, diagn_pm = FALSE, diagn_pp = FALSE; if (wxm > 0) { axial_xm = TRUE; } else if ( wmm > 0) { diagn_mm = TRUE; } else if (wmp > 0) { diagn_mp = TRUE; } if (wxp > 0) { axial_xp = TRUE; } else if ( wpm > 0) { diagn_pm = TRUE; } else if (wpp > 0) { diagn_pp = TRUE; } if (wym > 0) { axial_ym = TRUE; } else if ( wmm > 0) { diagn_mm = TRUE; } else if (wpm > 0) { diagn_pm = TRUE; } if (wyp > 0) { axial_yp = TRUE; } else if ( wmp > 0) { diagn_mp = TRUE; } else if (wpp > 0) { diagn_pp = TRUE; } int num_eqs = axial_xm + axial_xp + axial_ym + axial_yp + diagn_mm + diagn_mp + diagn_pm + diagn_pp; if ( (num_eqs == 0) && full ) { wxm = (x <= 0 ? 0.0 : 1.0); dxm = (dxmm + dxmp)/2; axial_xm = (wxm > 0); wxp = (x >= NX_G ? 0.0 : 1.0); dxp = (dxpm + dxpp)/2; axial_xp = (wxp > 0); wym = (y <= 0 ? 0.0 : 1.0); dym = (dymm + dypm)/2; axial_ym = (wym > 0); wyp = (y >= NY_G ? 0.0 : 1.0); dyp = (dymp + dypp)/2; axial_yp = (wyp > 0); num_eqs = axial_xm + axial_xp + axial_ym + axial_yp; assert(num_eqs > 0); } double wtot = 0; if ( num_eqs > 0 ) { assert((1+num_eqs) <= MAXCOEFS ); assert(N < NXY_Z); ix[xy] = N; imgsys_equation_t *eqk = &(eq[N]); int nt = 0; /* Number of terms in equation. */ eqk->rhs = 0.0; eqk->ix[nt] = xy; eqk->cf[nt] = 1.00; nt++; if ( axial_xm ) { long int kxm = (x-1) + y*NX_Z; eqk->ix[nt] = kxm; eqk->cf[nt] = -wxm; eqk->rhs += +wxm*dxm; nt++; } if ( axial_xp ) { long int kxp = (x+1) + y*NX_Z; eqk->ix[nt] = kxp; eqk->cf[nt] = -wxp; eqk->rhs += -wxp*dxp; nt++; } if ( axial_ym) { long int kym = x + (y-1)*NX_Z; eqk->ix[nt] = kym; eqk->cf[nt] = -wym; eqk->rhs += +wym*dym; nt++; } if ( axial_yp ) { long int kyp = x + (y+1)*NX_Z; eqk->ix[nt] = kyp; eqk->cf[nt] = -wyp; eqk->rhs += -wyp*dyp; nt++; } if ( diagn_mm ) { long int kmm = (x-1) + (y-1)*NX_Z; eqk->ix[nt] = kmm; eqk->cf[nt] = -wmm; eqk->rhs += wmm*(+ dxmm + dymm); nt++; } if ( diagn_mp ) { long int kmp = (x-1) + (y+1)*NX_Z; eqk->ix[nt] = kmp; eqk->cf[nt] = -wmp; eqk->rhs += wmp*(+ dxmp - dymp); nt++; } if ( diagn_pm ) { long int kpm = (x+1) + (y-1)*NX_Z; eqk->ix[nt] = kpm; eqk->cf[nt] = -wpm; eqk->rhs += wpm*(- dxpm + dypm); nt++; } if ( diagn_pp ) { long int kpp = (x+1) + (y+1)*NX_Z; eqk->ix[nt] = kpp; eqk->cf[nt] = -wpp; eqk->rhs += wpp*(- dxpp - dypp); nt++; } assert(nt == 1 + num_eqs); eqk->nt = nt; N++; /* Compute wtot and normalize coeficients: */ int j; wtot = 0; for(j = 1; j < nt; j++){ assert(eqk->cf[j] < 0); wtot+= -eqk->cf[j]; } assert(wtot > 0); for(j = 1; j < nt; j++){ eqk->cf[j]/=wtot; } eqk->rhs /= wtot; } else { assert(! full); ix[xy] = -1; } if (ZW != NULL) { set_sample(ZW, 0, x, y, wtot); } } /* Now replace the temporary indices in the equations by the correct ones, eliminating the excluded vars: */ long int k; for (k = 0; k < N; k++) { imgsys_equation_t *eqk = &(eq[k]); int nt = eqk->nt; int mt = 0; int i; for(i = 0; i < nt; i++) { /* Get the temporay index {xyi}: */ int xyi = eqk->ix[i]; /* Get the definitive index {ki}: */ int ki = ix[xyi]; if (ki >= 0) { /* Append the term to the equation: */ int j = mt; eqk->ix[j] = ki; eqk->cf[j] = eqk->cf[i]; assert(!isnan(eqk->cf[j])); mt++; } } eqk->nt = mt; } /* Build the inverse tables {col[0..N-1],row[0..N-1]}: */ int *col = (int *)notnull(malloc(N*sizeof(int)), "no mem"); int *row = (int *)notnull(malloc(N*sizeof(int)), "no mem"); { long int xy; for (xy = 0; xy < NXY_Z; xy++) { long int k = ix[xy]; if (k >= 0) { col[k] = xy % NX_Z; row[k] = xy / NX_Z; } } } /* Now package the equations as a system: */ imgsys_t *S = pst_imgsys_from_eqs(NX_Z, NY_Z, N, eq, ix, col, row); return S; } imgsys_t *pst_imgsys_from_eqs(int NX, int NY, long int N, imgsys_equation_t *eq, long int *ix, int *col, int *row) { imgsys_t *S = (imgsys_t*)malloc(sizeof(imgsys_t)); S->N = N; S->eq = eq; S->ix = ix; S->col = col; S->row = row; return S; } void pst_slope_map_solve_system ( imgsys_t *S, float_image_t *OZ, float_image_t *OW, long int maxIter, double convTol, int para, int szero, bool_t verbose, int level, long int reportIter, pst_height_map_report_proc_t *reportHeights ) { int indent = 2*level+2; long int *order = pst_imgsys_sort_equations(S,OW); auto void reportSol(long int iter, int change, bool_t final, long int N, double Z[]); /* A procedure that is called by the Gauss-Seidel solver at each iteration. When appropriate, it copies {Z} into the image {OZ} and calls {reportHeights}. This happens when {final} is TRUE or when {reportIter != 0} and {iter} is a multiple of {reportIter}. */ void reportSol(long int iter, int change, bool_t final, long int N, double Z[]) { bool_t doit = final || ((reportHeights != NULL) && (reportIter != 0) && (iter % reportIter == 0)); if (doit) { pst_slope_map_copy_sol_vec_to_height_map(S, Z, OZ); if (reportHeights != NULL) { reportHeights(level, iter, change, final, OZ, OW); } } } double *Z = (double*)malloc(sizeof(double)*S->N); pst_slope_map_copy_height_map_to_sol_vec(S, OZ, Z); pst_imgsys_solve(S, Z,order, maxIter, convTol, para, szero, verbose, indent, reportSol); /* {reportSol} must have copied the final solution into {OZ}. */ free(Z); free(order); } long int* pst_imgsys_sort_equations(imgsys_t *S, float_image_t *OW) { #define MAXDEGREE (2*((MAXCOEFS)-1)) // #define MAXDEGREE (2*((5)-1)) int N = S->N; // assert(MAXCOEFS <= 20); // assert(MAXCOEFS_v <=5); /* Build the graph of the equations */ long int *graph = (long int *)notnull(malloc(MAXDEGREE*N*sizeof(long int)), "no mem"); /* Value of out_degree[k] is the number of neighbours of pixel[k] with weight less than OW[k] */ long int *out_degree = (long int *)notnull(malloc(N*sizeof(long int)), "no mem"); /* Value of in_degree[k] is the number of unprocessed neighbours of pixel[k] with weight greater than OW[k] */ long int *in_degree = (long int *)notnull(malloc(N*sizeof(long int)), "no mem"); auto bool_t arrow( long int k1, long int k2); bool_t arrow( long int k1, long int k2){ double w1 = float_image_get_sample(OW,0,S->col[k1],S->row[k1]); double w2 = float_image_get_sample(OW,0,S->col[k2],S->row[k2]); return w1 < w2; } int k; for(k = 0; k < N; k++){ in_degree[k] = 0; out_degree[k] = 0; } for(k = 0; k < N; k++){ imgsys_equation_t *eqk = &(S->eq[k]); assert( eqk->ix[0] == k); int j; for(j = 1; j < eqk->nt; j++) { long int i = eqk->ix[j]; if(arrow(k,i)){ in_degree[i]++; graph[MAXDEGREE*k + out_degree[k]] = i; out_degree[k]++;} if(arrow(i,k)){ in_degree[k]++; graph[MAXDEGREE*i + out_degree[i]] = k; out_degree[i]++;} } } long int queue_free = 0; long int queue_start = 0; long int *queue = (long int *)notnull(malloc(N*sizeof(long int)), "no mem"); auto void eq_queue_insert(long int index); void eq_queue_insert(long int index){ assert(queue_free < N); queue[queue_free] = index; queue_free++; } auto long int eq_queue_remove( void ); long int eq_queue_remove( void ){ assert(queue_start < queue_free ); long int i = queue[queue_start]; queue_start++; return i; } for(k = 0; k < N; k++) {if( in_degree[k] == 0){ eq_queue_insert(k); }} while(queue_start < queue_free ) { long int k = eq_queue_remove(); int j; for(j = 0; j < out_degree[k]; j++) { long int i = graph[MAXDEGREE*k + j]; assert(in_degree[i] > 0); in_degree[i]--; if(in_degree[i] == 0){ eq_queue_insert(i); } } } assert(queue_free == N); free(graph); free(in_degree); free(out_degree); return queue; }