/*
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);
}
