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;
  
}
Exemplo n.º 2
0
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;
  
}