/* Calculate interpolation coefficients for Linear */ Matrix<double> setupSplineLinear(const Vector<double>& x, const Vector<double>& y) { const unsigned int n = x.size(); Matrix<double> interp_coeffs(2, n - 1); for (unsigned int i = 0; i < n - 1; ++i) { interp_coeffs(0, i) = y(i); interp_coeffs(1, i) = (y(i + 1) - y(i)) / (x(i + 1) - x(i)); } return interp_coeffs; }
/* Calculate interpolation coefficients for Monotone splines */ Matrix<double> setupSplineMonotoneSpline(const Vector<double>& x, const Vector<double>& y) { // these are the interpolation coefficients // y(x, x_i-1<x<x_i) = _ai (x - x_i)^3 + _bi (x - x_i)^2 // + _ci (x - x_i) + _di const unsigned int n = x.size(); Matrix<double> interp_coeffs(4, n - 1); // initialize x-grid step sizes and slopes. Vector<double> h(n - 1); Vector<double> s(n - 1); for (unsigned int i = 0; i < n - 1; ++i) { h(i) = x(i + 1) - x(i); s(i) = (y(i + 1) - y(i)) / h(i); } if (n > 2) { Vector<double> p(n); p(0) = 0.0; p(n - 1) = 0.0; for (unsigned int i = 1; i < n - 1; ++i) p(i) = (s(i - 1) * h(i) + s(i) * h(i - 1)) / (h(i - 1) + h(i)); Vector<double> yp(n); for (unsigned int i = 1; i < n - 1; ++i) { double s1 = abs(s(i - 1)); double s2 = abs(s(i)); if (s(i - 1) * s(i) <= 0.0) { yp(i) = 0.0; } else if ((abs(p(i)) > 2.0 * s1) || (abs(p(i)) > 2.0 * s2)) { double a = (s1 > 0.0) - (s1 < 0.0); //sign function. yp(i) = 2.0 * a * min(s1, s2); } else { yp(i) = p(i); } } // second derivative at extreme points equal // to zero boundary condition. yp(0) = 1.5 * s(0) - 0.5 * yp(1); yp(n - 1) = 1.5 * s(n - 2) - 0.5 * yp(n - 3); for (unsigned int i = 0; i < n - 1; ++i) { interp_coeffs(3, i) = (yp(i) + yp(i + 1) - 2.0 * s(i)) / (h(i) * h(i)); interp_coeffs(2, i) = (3.0 * s(i) - 2.0 * yp(i) - yp(i + 1)) / h(i); interp_coeffs(1, i) = yp(i); interp_coeffs(0, i) = y(i); } } else { // in this case, with second derivative at extreme points equal to zero, // the solution will be just the linear interpolation. interp_coeffs(3, 0) = 0.0; interp_coeffs(3, 1) = 0.0; interp_coeffs(2, 0) = 0.0; interp_coeffs(2, 1) = 0.0; interp_coeffs(1, 0) = s(0); interp_coeffs(1, 1) = s(1); interp_coeffs(0, 0) = y(0); interp_coeffs(0, 1) = y(1); } return interp_coeffs; }
void *thread_func(void* threadarg) { /* Get struct argument */ struct thread_data *my_data; my_data = (struct thread_data *) threadarg; int thread_id = my_data->thread_id; int top = my_data->top; int bottom = my_data->bottom; int left = my_data->left; int right = my_data->right; int subimage = my_data->subimage; double* Ai = my_data->Ai; double* w1w2 = my_data->w1w2; double* w2 = my_data->w2; double* f1f2 = my_data->f1f2; double* f2 = my_data->f2; double* mu1 = my_data->mu1; double* mu2 = my_data->mu2; double* exp_sqr_err = my_data->exp_sqr_err; double* Bi = my_data->Bi; int *xjfloor = my_data->xjfloor; int *yjfloor = my_data->yjfloor; double *coeffs = my_data->coeffs; double erri; double sqrcoeffs[n_interp]; /* Loop over all blurry image pixels: (xi,yi): 1-based 2D position in blurry image */ int xi, yi; int i, j, k, jj, kjj; double sqrBik, sqrAij, w2inc, term2i, term3i, term4i; double xj, yj; int xinterp, yinterp; bool inbounds; /* Loop over all blurry pixels i */ for(xi=left; xi<=right; ++xi) { for(yi=top; yi<=bottom; ++yi) { i = (xi-1+blurry_subim_l[subimage]-1)*h_blurry + yi - 1; if(obsmask[i]) ++(*mu2); /* Increment observation count */ else continue; /* Skip this blurry pixel */ term2i = 0; term3i = 0; term4i = 0; /* Clear temporary variables */ double gbari = 0; /* Clear Ai */ for(j=0; j<n_sharp; ++j) Ai[j] = 0; /* Loop over all orientations k */ for(k=0; k<n_kernel; ++k) { Bi[k] = 0; /* Project blurry pixel into sharp image */ project_blurry_to_sharp(yi,xi,&Hcache[k*9],&xj,&yj); /* Get interpolation coefficients */ interp_coeffs(xj,yj,&xjfloor[k],&yjfloor[k],&coeffs[k*n_interp]); for(jj=0; jj<n_interp; ++jj) sqrcoeffs[jj] = sqr(coeffs[k*n_interp+jj]); /* Interpolate points */ for(jj=0; jj<n_interp; ++jj) { yinterp = yjfloor[k] + yoff[jj]; xinterp = xjfloor[k] + xoff[jj]; inbounds = yinterp >= 1 && yinterp <= h_sharp && xinterp >= 1 && xinterp <= w_sharp; if(!inbounds) continue; j = (xinterp-1+sharp_subim_l[subimage]-1)*h_sharp + yinterp - 1; kjj = k*n_interp+jj; Bi[k] += coeffs[kjj]*mf1[j]; Ai[j] += coeffs[kjj]*mw1[k]; f2[j] += sqrcoeffs[jj]*var_w[k]; w2inc = sqrcoeffs[jj]*var_f[j]; w2[k] += w2inc; term2i += w2inc*var_w[k]; /* += sqrcoeffs[jj]*var_f[j]*var_w[k]; */ } sqrBik = Bi[k]*Bi[k]; /* w2 */ w2[k] += sqrBik; /* Accumulate gbar[i] */ gbari += Bi[k]*mw1[k]; /* exp_sqr_err */ term4i += sqrBik*var_w[k]; /* Having calculated Bik, interpolate terms involving it */ for(jj=0; jj<n_interp; ++jj) { yinterp = yjfloor[k] + yoff[jj]; xinterp = xjfloor[k] + xoff[jj]; inbounds = yinterp >= 1 && yinterp <= h_sharp && xinterp >= 1 && xinterp <= w_sharp; if(!inbounds) continue; j = (xinterp-1+sharp_subim_l[subimage]-1)*h_sharp + yinterp - 1; f1f2[j] += coeffs[k*n_interp+jj]*Bi[k]*var_w[k]; } } /* Expected error */ erri = obsmask[i]*(g[i] - gbari - mmu1); *mu1 += obsmask[i]*(g[i] - gbari); /* Accumulate terms for image parameters */ for(j=0; j<n_sharp; ++j) { if(Ai[j] > 0) { sqrAij = Ai[j]*Ai[j]; term3i += sqrAij*var_f[j]; f2[j] += sqrAij; f1f2[j] += Ai[j]*erri; } } /* Loop over rotations again to calculate terms for kernel */ for(k=0; k<n_kernel; ++k) { /* Interpolate points */ for(jj=0; jj<n_interp; ++jj) { yinterp = yjfloor[k] + yoff[jj]; xinterp = xjfloor[k] + xoff[jj]; inbounds = yinterp >= 1 && yinterp <= h_sharp && xinterp >= 1 && xinterp <= w_sharp; if(!inbounds) continue; j = (xinterp-1+sharp_subim_l[subimage]-1)*h_sharp + yinterp - 1; w1w2[k] -= coeffs[k*n_interp+jj]*Ai[j]*var_f[j]; } /* w1w2 */ w1w2[k] += Bi[k]*erri; } /* Add everything together for exp_sqr_err */ exp_sqr_err[i] = sqr(erri) + term2i + term3i + term4i + mmu2 - sqr(mmu1); } } pthread_exit(NULL); }