real StressMajorizationSmoother_smooth(StressMajorizationSmoother sm, int dim, real *x, int maxit_sm, real tol) {
SparseMatrix Lw = sm->Lw, Lwd = sm->Lwd, Lwdd = NULL;
int i, j, k, m, *id, *jd, *iw, *jw, idiag, flag = 0, iter = 0;
real *w, *dd, *d, *y = NULL, *x0 = NULL, *x00 = NULL, diag, diff = 1, *lambda = sm->lambda, res, alpha = 0., M = 0.;
SparseMatrix Lc = NULL;
real dij, dist;
Lwdd = SparseMatrix_copy(Lwd);
m = Lw->m;
x0 = N_GNEW(dim*m,real);
if (!x0) goto RETURN;
x0 = MEMCPY(x0, x, sizeof(real)*dim*m);
y = N_GNEW(dim*m,real);
if (!y) goto RETURN;
id = Lwd->ia; jd = Lwd->ja;
d = (real*) Lwd->a;
dd = (real*) Lwdd->a;
w = (real*) Lw->a;
iw = Lw->ia; jw = Lw->ja;
#ifdef DEBUG_PRINT
if (Verbose) fprintf(stderr, "initial stress = %f\n", get_stress(m, dim, iw, jw, w, d, x, sm->scaling, sm->data, 1));
#endif
/* for the additional matrix L due to the position constraints */
if (sm->scheme == SM_SCHEME_NORMAL_ELABEL){
get_edge_label_matrix(sm->data, m, dim, x, &Lc, &x00);
if (Lc) Lw = SparseMatrix_add(Lw, Lc);
} else if (sm->scheme == SM_SCHEME_UNIFORM_STRESS){
alpha = ((real*) (sm->data))[0];
M = ((real*) (sm->data))[1];
}
while (iter++ < maxit_sm && diff > tol){
#ifdef GVIEWER
if (Gviewer) {
drawScene();
if (iter%2 == 0) gviewer_dump_current_frame();
}
#endif
if (sm->scheme != SM_SCHEME_STRESS_APPROX){
for (i = 0; i < m; i++){
idiag = -1;
diag = 0.;
for (j = id[i]; j < id[i+1]; j++){
if (i == jd[j]) {
idiag = j;
continue;
}
dist = distance(x, dim, i, jd[j]);
//if (d[j] >= -0.0001*dist){
// /* sometimes d[j] = 0 and ||x_i-x_j||=0*/
// dd[j] = d[j]/MAX(0.0001, dist);
if (d[j] == 0){
dd[j] = 0;
} else {
if (dist == 0){
dij = d[j]/w[j];/* the ideal distance */
/* perturb so points do not sit at the same place */
for (k = 0; k < dim; k++) x[jd[j]*dim+k] += 0.0001*(drand()+.0001)*dij;
dist = distance(x, dim, i, jd[j]);
}
dd[j] = d[j]/dist;
#if 0
/* if two points are at the same place, we do not want a huge entry,
as this cause problem with CG./ In any case,
at thw limit d[j] == ||x[i] - x[jd[j]]||,
or close. */
if (dist < -d[j]*0.0000001){
dd[j] = -10000.;
} else {
dd[j] = d[j]/dist;
}
#endif
}
diag += dd[j];
}
assert(idiag >= 0);
dd[idiag] = -diag;
}
/* solve (Lw+lambda*I) x = Lwdd y + lambda x0 */
SparseMatrix_multiply_dense(Lwdd, FALSE, x, FALSE, &y, FALSE, dim);
} else {
for (i = 0; i < m; i++){
for (j = 0; j < dim; j++){
y[i*dim+j] = 0;/* for stress_approx scheme, the whole rhs is calculated in stress_maxent_augment_rhs */
}
}
}
if (lambda){/* is there a penalty term? */
for (i = 0; i < m; i++){
for (j = 0; j < dim; j++){
y[i*dim+j] += lambda[i]*x0[i*dim+j];
}
}
}
/* additional term added to the rhs */
switch (sm->scheme){
case SM_SCHEME_NORMAL_ELABEL: {
for (i = 0; i < m; i++){
for (j = 0; j < dim; j++){
y[i*dim+j] += x00[i*dim+j];
}
}
break;
}
case SM_SCHEME_UNIFORM_STRESS:{/* this part can be done more efficiently using octree approximation */
uniform_stress_augment_rhs(m, dim, x, y, alpha, M);
break;
}
#if UNIMPEMENTED
case SM_SCHEME_MAXENT:{
#ifdef GVIEWER
if (Gviewer){
char *lab;
lab = MALLOC(sizeof(char)*100);
sprintf(lab,"maxent. alpha=%10.2g, iter=%d",stress_maxent_data_get_alpha(sm->data), iter);
gviewer_set_label(lab);
FREE(lab);
}
#endif
stress_maxent_augment_rhs_fast(sm, dim, x, y, &flag);
if (flag) goto RETURN;
break;
}
case SM_SCHEME_STRESS_APPROX:{
stress_approx_augment_rhs(sm, dim, x, y, &flag);
if (flag) goto RETURN;
break;
}
case SM_SCHEME_STRESS:{
#ifdef GVIEWER
if (Gviewer){
char *lab;
lab = MALLOC(sizeof(char)*100);
sprintf(lab,"pmds(k), iter=%d", iter);
gviewer_set_label(lab);
FREE(lab);
}
#endif
}
#endif /* UNIMPEMENTED */
default:
break;
}
#ifdef DEBUG_PRINT
if (Verbose) {
fprintf(stderr, "stress1 = %g\n",get_stress(m, dim, iw, jw, w, d, x, sm->scaling, sm->data, 1));
}
#endif
if (sm->scheme == SM_SCHEME_UNIFORM_STRESS){
res = uniform_stress_solve(Lw, alpha, dim, x, y, sm->tol_cg, sm->maxit_cg, &flag);
} else {
res = SparseMatrix_solve(Lw, dim, x, y, sm->tol_cg, sm->maxit_cg, SOLVE_METHOD_CG, &flag);
//res = SparseMatrix_solve(Lw, dim, x, y, sm->tol_cg, 1, SOLVE_METHOD_JACOBI, &flag);
}
if (flag) goto RETURN;
#ifdef DEBUG_PRINT
if (Verbose) fprintf(stderr, "stress2 = %g\n",get_stress(m, dim, iw, jw, w, d, y, sm->scaling, sm->data, 1));
#endif
diff = total_distance(m, dim, x, y)/sqrt(vector_product(m*dim, x, x));
#ifdef DEBUG_PRINT
if (Verbose){
fprintf(stderr, "Outer iter = %d, cg res = %g, ||x_{k+1}-x_k||/||x_k|| = %g\n",iter, res, diff);
}
#endif
MEMCPY(x, y, sizeof(real)*m*dim);
}
#ifdef DEBUG
_statistics[1] += iter-1;
#endif
#ifdef DEBUG_PRINT
if (Verbose) fprintf(stderr, "iter = %d, final stress = %f\n", iter, get_stress(m, dim, iw, jw, w, d, x, sm->scaling, sm->data, 1));
#endif
RETURN:
SparseMatrix_delete(Lwdd);
if (Lc) {
SparseMatrix_delete(Lc);
SparseMatrix_delete(Lw);
}
if (x0) FREE(x0);
if (y) FREE(y);
if (x00) FREE(x00);
return diff;
}
real StressMajorizationSmoother_smooth(StressMajorizationSmoother sm, int dim, real *x, int maxit_sm, real tol) {
SparseMatrix Lw = sm->Lw, Lwd = sm->Lwd, Lwdd = NULL;
int i, j, m, *id, *jd, *iw, *jw, idiag, flag = 0, iter = 0;
real *w, *dd, *d, *y = NULL, *x0 = NULL, *x00 = NULL, diag, diff = 1, *lambda = sm->lambda, maxit, res, alpha = 0., M = 0.;
SparseMatrix Lc = NULL;
Lwdd = SparseMatrix_copy(Lwd);
m = Lw->m;
x0 = N_GNEW(dim*m,real);
if (!x0) goto RETURN;
x0 = MEMCPY(x0, x, sizeof(real)*dim*m);
y = N_GNEW(dim*m,real);
if (!y) goto RETURN;
id = Lwd->ia; jd = Lwd->ja;
d = (real*) Lwd->a;
dd = (real*) Lwdd->a;
w = (real*) Lw->a;
iw = Lw->ia; jw = Lw->ja;
/* for the additional matrix L due to the position constraints */
if (sm->scheme == SM_SCHEME_NORMAL_ELABEL){
get_edge_label_matrix(sm->data, m, dim, x, &Lc, &x00);
if (Lc) Lw = SparseMatrix_add(Lw, Lc);
} else if (sm->scheme == SM_SCHEME_UNIFORM_STRESS){
alpha = ((real*) (sm->data))[0];
M = ((real*) (sm->data))[1];
}
while (iter++ < maxit_sm && diff > tol){
for (i = 0; i < m; i++){
idiag = -1;
diag = 0.;
for (j = id[i]; j < id[i+1]; j++){
if (i == jd[j]) {
idiag = j;
continue;
}
dd[j] = d[j]/distance_cropped(x, dim, i, jd[j]);
diag += dd[j];
}
assert(idiag >= 0);
dd[idiag] = -diag;
}
/* solve (Lw+lambda*I) x = Lwdd y + lambda x0 */
SparseMatrix_multiply_dense(Lwdd, FALSE, x, FALSE, &y, FALSE, dim);
if (lambda){/* is there a penalty term? */
for (i = 0; i < m; i++){
for (j = 0; j < dim; j++){
y[i*dim+j] += lambda[i]*x0[i*dim+j];
}
}
}
if (sm->scheme == SM_SCHEME_NORMAL_ELABEL){
for (i = 0; i < m; i++){
for (j = 0; j < dim; j++){
y[i*dim+j] += x00[i*dim+j];
}
}
} else if (sm->scheme == SM_SCHEME_UNIFORM_STRESS){/* this part can be done more efficiently using octree approximation */
uniform_stress_augment_rhs(m, dim, x, y, alpha, M);
}
#ifdef DEBUG_PRINT
if (Verbose) fprintf(stderr, "stress1 = %f\n",get_stress(m, dim, iw, jw, w, d, x, sm->scaling, sm->data, 0));
#endif
maxit = sqrt((double) m);
if (sm->scheme == SM_SCHEME_UNIFORM_STRESS){
res = uniform_stress_solve(Lw, alpha, dim, x, y, 0.01, maxit, &flag);
} else {
res = SparseMatrix_solve(Lw, dim, x, y, 0.01, maxit, SOLVE_METHOD_CG, &flag);
}
if (flag) goto RETURN;
#ifdef DEBUG_PRINT
if (Verbose) fprintf(stderr, "stress2 = %f\n",get_stress(m, dim, iw, jw, w, d, y, sm->scaling, sm->data, 0));
#endif
diff = total_distance(m, dim, x, y)/sqrt(vector_product(m*dim, x, x));
#ifdef DEBUG_PRINT
if (Verbose){
fprintf(stderr, "Outer iter = %d, res = %g Stress Majorization diff = %g\n",iter, res, diff);
}
#endif
MEMCPY(x, y, sizeof(real)*m*dim);
}
#ifdef DEBUG
_statistics[1] += iter-1;
#endif
RETURN:
SparseMatrix_delete(Lwdd);
if (Lc) {
SparseMatrix_delete(Lc);
SparseMatrix_delete(Lw);
}
if (x0) FREE(x0);
if (y) FREE(y);
if (x00) FREE(x00);
return diff;
}