DenseGrid3D<float> get_rasterized(const Gaussian3Ds &gmm, const Floats &weights,
double cell_width, const BoundingBox3D &bb) {
DenseGrid3D<float> ret(cell_width, bb, 0);
for (unsigned int ng = 0; ng < gmm.size(); ng++) {
IMP_Eigen::Matrix3d covar = get_covariance(gmm[ng]);
// suppress warning
IMP_Eigen::Matrix3d inverse = IMP_Eigen::Matrix3d::Zero(3, 3);
double determinant;
bool invertible;
covar.computeInverseAndDetWithCheck(inverse, determinant, invertible);
double pre = 1.0 / pow(2 * algebra::PI, 2.0 / 3.0) / std::sqrt(determinant);
if (!invertible || determinant < 0) {
std::cout << "\n\n\n->>>>not proper matrix!!\n\n\n" << std::endl;
}
IMP_Eigen::Vector3d center(gmm[ng].get_center().get_data());
IMP_INTERNAL_CHECK(invertible, "matrix wasn't invertible! uh oh!");
IMP_FOREACH(const DenseGrid3D<float>::Index & i, ret.get_all_indexes()) {
Vector3D aloc = ret.get_center(i);
IMP_Eigen::Vector3d loc(aloc[0], aloc[1], aloc[2]);
IMP_Eigen::Vector3d r = loc - center;
double d = r.transpose() * (inverse * r);
double score = pre * weights[ng] * std::exp(-0.5 * (d));
if (score > 1e10) {
score = 100;
}
if (score > 0) {
ret[i] += score;
}
}
}
return ret;
}
DensityGrid get_rasterized_fast(const Gaussian3Ds &gmm, const Floats &weights,
double cell_width, const BoundingBox3D &bb, double factor) {
DensityGrid ret(cell_width, bb, 0);
for (unsigned int ng = 0; ng < gmm.size(); ng++) {
Eigen::Matrix3d covar = get_covariance(gmm[ng]);
Eigen::Matrix3d inverse = Eigen::Matrix3d::Zero(3, 3);
double determinant;
bool invertible;
covar.computeInverseAndDetWithCheck(inverse, determinant, invertible);
IMP_INTERNAL_CHECK((invertible && determinant > 0),
"Tried to invert Gaussian, but it's not proper matrix");
double pre(get_gaussian_eval_prefactor(determinant));
Eigen::Vector3d evals = covar.eigenvalues().real();
double maxeval = sqrt(evals.maxCoeff());
double cutoff = factor * maxeval;
double cutoff2 = cutoff * cutoff;
Vector3D c = gmm[ng].get_center();
Vector3D lower = c - Vector3D(cutoff, cutoff, cutoff);
Vector3D upper = c + Vector3D(cutoff, cutoff, cutoff);
GridIndex3D lowerindex = ret.get_nearest_index(lower);
GridIndex3D upperindex = ret.get_nearest_index(upper);
Eigen::Vector3d center(c.get_data());
IMP_INTERNAL_CHECK(invertible, "matrix wasn't invertible! uh oh!");
IMP_GRID3D_FOREACH_SMALLER_EXTENDED_INDEX_RANGE(ret, upperindex, lowerindex,
upperindex, {
GridIndex3D i(voxel_index[0], voxel_index[1], voxel_index[2]);
Eigen::Vector3d r(get_vec_from_center(i, ret, center));
if (r.squaredNorm() < cutoff2) {
update_value(&ret, i, r, inverse, pre, weights[ng]);
}
})
}
return ret;
}
DensityGrid get_rasterized(const Gaussian3Ds &gmm, const Floats &weights,
double cell_width, const BoundingBox3D &bb) {
DensityGrid ret(cell_width, bb, 0);
for (unsigned int ng = 0; ng < gmm.size(); ng++) {
Eigen::Matrix3d covar = get_covariance(gmm[ng]);
Eigen::Matrix3d inverse = Eigen::Matrix3d::Zero(3, 3);
double determinant;
bool invertible;
covar.computeInverseAndDetWithCheck(inverse, determinant, invertible);
IMP_INTERNAL_CHECK((invertible && determinant > 0),
"Tried to invert Gaussian, but it's not proper matrix");
double pre(get_gaussian_eval_prefactor(determinant));
Eigen::Vector3d center(gmm[ng].get_center().get_data());
IMP_INTERNAL_CHECK(invertible, "matrix wasn't invertible! uh oh!");
IMP_FOREACH(const DensityGrid::Index & i, ret.get_all_indexes()) {
Eigen::Vector3d r(get_vec_from_center(i, ret, center));
update_value(&ret, i, r, inverse, pre, weights[ng]);
}
}
return ret;
}
static double getSSErr(const VARIOGRAM *vp, PERM *p, LM *lm) {
int i;
double x, y, z, dz, SSErr;
/*
if (fill_weights(vp, p, lm))
return 0.0;
*/
for (i = 0, SSErr = 0.0; i < p->size; i++) {
x = vp->ev->direction.x * vp->ev->dist[p->pe[i]];
y = vp->ev->direction.y * vp->ev->dist[p->pe[i]];
z = vp->ev->direction.z * vp->ev->dist[p->pe[i]];
/* fill y with current residuals: */
dz = vp->ev->gamma[p->pe[i]] - (is_variogram(vp) ?
get_semivariance(vp, x, y, z) :
get_covariance(vp, x, y, z));
if (lm->weights != NULL)
SSErr += dz * dz * lm->weights->ve[i];
else
SSErr += dz * dz;
}
return SSErr;
}
static double sem_cov_blocks(VARIOGRAM *v, DATA *a, DATA *b, int sem) {
/*
* Purpose : calculates (once) and returns Cov(a,b);
* if a==b && a denotes a block discretisation, the value
* is put in a->block_variance and a->block_xxx_set gets 1
* Created by : Edzer J. Pebesma
* Date : 25 jan 1992, 3 june 1993
* Prerequisites : none
* Returns : sem == 1 ? Sem(a,b) : Cov(a,b)
* Side effects : none
*/
int i, j;
double block_value, dx, dy = 0.0, dz = 0.0, dist, ret, weight, dzero2;
DPOINT *dpa, *dpb;
/*
if (a->what_is_u != U_ISWEIGHT || b->what_is_u != U_ISWEIGHT)
ErrMsg(ER_IMPOSVAL, "weights needed in SevCov_Blocks()");
*/
if (a == NULL)
return sem ? get_semivariance(v, 0.0, 0.0, 0.0) :
get_covariance(v, 0.0, 0.0, 0.0);
if (a->n_list == 1 && b->n_list == 1) { /* point--point */
if (gl_longlat) {
if (! v->isotropic)
ErrMsg(ER_IMPOSVAL, "for long/lat data, anisotropy cannot be defined");
dist = pp_norm_gc(a->list[0], b->list[0]);
ret = sem ? get_semivariance(v, dist, 0.0, 0.0):
get_covariance(v, dist, 0.0, 0.0);
/* printf("ll dist: %g, ret.val %g\n", dist, ret); */
return ret;
} else {
return sem ?
get_semivariance(v, a->list[0]->x - b->list[0]->x,
a->list[0]->y - b->list[0]->y, a->list[0]->z - b->list[0]->z):
get_covariance(v, a->list[0]->x - b->list[0]->x,
a->list[0]->y - b->list[0]->y, a->list[0]->z - b->list[0]->z);
}
}
/* now a->n_list > 1 or b->n_list > 1: block--block or point--block */
if (gl_longlat)
ErrMsg(ER_IMPOSVAL, "block kriging for long-lat data undefined");
if (a == b) { /* block--block for a single block */
if (sem && v->block_semivariance_set)
return v->block_semivariance;
if (!sem && v->block_covariance_set)
return v->block_covariance;
}
/* else: continue */
dzero2 = gl_zero * gl_zero;
block_value = 0.0;
for (i = 0; i < a->n_list; i++) {
for (j = 0; j < b->n_list; j++) { /* compare all points with all p */
dpa = a->list[i];
dpb = b->list[j];
weight = dpa->u.weight * dpb->u.weight;
/* avoid the "zero-effect" if a or b is block: */
dx = dpa->x - dpb->x;
dy = dpa->y - dpb->y;
dz = dpa->z - dpb->z;
/* avoid ``zero-effect'': */
if (a->pp_norm2(dpa, dpb) < dzero2) {
dx = (dx >= 0 ? gl_zero : -gl_zero);
if (a->mode & Y_BIT_SET)
dy = (dy >= 0 ? gl_zero : -gl_zero);
if (a->mode & Z_BIT_SET)
dz = (dz >= 0 ? gl_zero : -gl_zero);
}
if (sem)
block_value += weight * get_semivariance(v, dx, dy, dz);
else
block_value += weight * get_covariance(v, dx, dy, dz);
} /* for j */
} /* for i */
if (a == b) { /* remember within block cov./sem.: */
if (sem) {
v->block_semivariance = block_value;
v->block_semivariance_set = 1;
} else {
v->block_covariance = block_value;
v->block_covariance_set = 1;
}
}
return block_value;
}
int init_global_variables(void) {
/*
* global variables init. (glvars.h; defautls.h):
*/
method = NSP;
mode = MODE_NSP;
debug_level = DB_NORMAL;
gl_blas = DEF_blas;
gl_choleski = DEF_choleski;
gl_coincide = DEF_coincide;
gl_cressie = DEF_cressie;
gl_fit = DEF_fit;
gl_gauss = DEF_gauss;
gl_iter = DEF_iter;
gl_jgraph = DEF_jgraph;
gl_lhs = DEF_lhs;
gl_longlat = DEF_longlat;
gl_nblockdiscr = DEF_nblockdiscr;
gl_n_intervals = DEF_intervals;
gl_n_marginals = DEF_n_marginals;
gl_nsim = DEF_nsim;
gl_n_uk = DEF_n_uk;
gl_numbers = DEF_numbers;
gl_nocheck = DEF_nocheck;
gl_order = DEF_order;
gl_register_pairs = DEF_pairs;
gl_rowwise = DEF_rowwise;
gl_rp = DEF_rp;
gl_seed = DEF_seed;
gl_sim_beta = DEF_sim_beta;
gl_spiral = DEF_spiral;
gl_split = DEF_split;
gl_sym_ev = DEF_sym_ev;
gl_gls_residuals = DEF_gls_residuals;
gl_sparse = DEF_sparse;
gl_xvalid = DEF_xvalid;
gl_zero_est = DEF_zero_est;
gl_bounds = DEF_bounds;
gl_cutoff = DEF_cutoff;
gl_fit_limit = DEF_fit_limit;
gl_fraction = DEF_fraction;
gl_idp = DEF_idp;
gl_iwidth = DEF_iwidth;
gl_quantile = DEF_quantile;
gl_zmap = DEF_zmap;
gl_alpha = DEF_alpha;
gl_beta = DEF_beta;
gl_tol_hor = DEF_tol_hor;
gl_tol_ver = DEF_tol_ver;
gl_zero = DEF_zero;
init_gstat_data(0);
/* EJPXX
* if (valdata == NULL)
* */
valdata = init_one_data(valdata);
block.x = block.y = block.z = 0.0;
set_mv_double(&gl_zmap);
get_covariance(NULL, 0, 0, 0);
return 0;
}
static int fit_GaussNewton(VARIOGRAM *vp, PERM *p, LM *lm, int iter, int *bounded) {
double s = 0.0, x, y, z;
int i, j, n_fit, model, fit_ranges = 0;
IVEC *fit = NULL;
VEC *start = NULL;
if (p->size == 0)
return 1;
fit = iv_resize(fit, 2 * vp->n_models);
/* index fit parameters: parameter fit->ive[j] corresponds to model i */
for (i = n_fit = 0; i < vp->n_models; i++) {
if (vp->part[i].fit_sill)
fit->ive[n_fit++] = i;
if (vp->part[i].fit_range) {
fit->ive[n_fit++] = i + vp->n_models; /* large -->> ranges */
fit_ranges = 1;
}
}
if (n_fit == 0) {
iv_free(fit);
return 0;
}
fit = iv_resize(fit, n_fit); /* shrink to fit */
lm->X = m_resize(lm->X, p->size, n_fit);
lm->y = v_resize(lm->y, p->size);
start = v_resize(start, n_fit);
for (i = 0; i < n_fit; i++) {
if (fit->ive[i] < vp->n_models) {
model = fit->ive[i];
start->ve[i] = vp->part[model].sill;
} else {
model = fit->ive[i] - vp->n_models;
start->ve[i] = vp->part[model].range[0];
}
}
for (i = 0; i < p->size; i++) {
x = vp->ev->direction.x * vp->ev->dist[p->pe[i]];
y = vp->ev->direction.y * vp->ev->dist[p->pe[i]];
z = vp->ev->direction.z * vp->ev->dist[p->pe[i]];
/* fill y with current residuals: */
if (is_variogram(vp))
s = get_semivariance(vp, x, y, z);
else
s = get_covariance(vp, x, y, z);
lm->y->ve[i] = vp->ev->gamma[p->pe[i]] - s;
/* fill X: */
for (j = 0; j < n_fit; j++) { /* cols */
if (fit->ive[j] < vp->n_models) {
model = fit->ive[j];
ME(lm->X, i, j) = (is_variogram(vp) ?
UnitSemivariance(vp->part[model],x,y,z) :
UnitCovariance(vp->part[model],x,y,z));
} else {
model = fit->ive[j] - vp->n_models;
ME(lm->X, i, j) = (is_variogram(vp) ?
da_Semivariance(vp->part[model],x,y,z) :
-da_Semivariance(vp->part[model],x,y,z));
}
}
}
if (iter == 0 && fill_weights(vp, p, lm)) {
iv_free(fit);
v_free(start);
return 1;
}
lm->has_intercept = 1; /* does not affect the fit */
lm = calc_lm(lm); /* solve WLS eqs. for beta */
if (DEBUG_FIT) {
Rprintf("beta: ");
v_logoutput(lm->beta);
}
if (lm->is_singular) {
iv_free(fit);
v_free(start);
return 1;
}
if (fit_ranges) {
s = v_norm2(lm->beta) / v_norm2(start);
if (s > 0.2) {
/* don't allow steps > 20% ---- */
sv_mlt(0.2 / s, lm->beta, lm->beta);
*bounded = 1;
} else
*bounded = 0; /* a `free', voluntary step */
} else /* we're basically doing linear regression here: */
*bounded = 0;
for (i = 0; i < n_fit; i++) {
if (fit->ive[i] < vp->n_models) {
model = fit->ive[i];
vp->part[model].sill = start->ve[i] + lm->beta->ve[i];
} else {
model = fit->ive[i] - vp->n_models;;
vp->part[model].range[0] = start->ve[i] + lm->beta->ve[i];
}
}
iv_free(fit);
v_free(start);
return 0;
}
