Exemple #1
0
// Standard CG method starting from zero vector
// (because we solve for the increment)
// x... comes as right-hand side, leaves as solution
bool CommonSolverCG::_solve(Matrix* A, double *x, double tol, int maxiter)
{
    printf("CG solver\n");

    int n_dof = A->get_size();
    double *r = new double[n_dof];
    double *p = new double[n_dof];
    double *help_vec = new double[n_dof];
    if (r == NULL || p == NULL || help_vec == NULL) {
        _error("a vector could not be allocated in solve_linear_system_iter().");
    }
    // r = b - A*x0  (where b is x and x0 = 0)
    for (int i=0; i < n_dof; i++) r[i] = x[i];
    // p = r
    for (int i=0; i < n_dof; i++) p[i] = r[i];

    // setting initial condition x = 0
    for (int i=0; i < n_dof; i++) x[i] = 0;

    // CG iteration
    int iter_current = 0;
    double tol_current;
    while (1)
    {
        mat_dot(A, p, help_vec, n_dof);
        double r_times_r = vec_dot(r, r, n_dof);
        double alpha = r_times_r / vec_dot(p, help_vec, n_dof);
        for (int i=0; i < n_dof; i++) {
            x[i] += alpha*p[i];
            r[i] -= alpha*help_vec[i];
        }
        double r_times_r_new = vec_dot(r, r, n_dof);
        iter_current++;
        tol_current = sqrt(r_times_r_new);
        if (tol_current < tol
            || iter_current >= maxiter) break;
        double beta = r_times_r_new/r_times_r;
        for (int i=0; i < n_dof; i++) p[i] = r[i] + beta*p[i];
    }
    bool flag;
    if (tol_current <= tol)
        flag = true;
    else
        flag = false;

    if (r != NULL) delete [] r;
    if (p != NULL) delete [] p;
    if (help_vec != NULL) delete [] help_vec;

    printf("CG solver: maxiter: %i, tol: %e\n",
           iter_current, tol_current);

    return flag;
}
      virtual Eigen::Matrix<double,6,6> get_xyz_vel()
      {
	Eigen::Matrix3d mat = get();
	Eigen::Matrix3d mat_dot = get_dot();
	Eigen::Matrix<double,6,6> retval;
	
	for (register int i = 0; i < 3; ++i) {
	  for (register int j = 0; j < 3; ++j) {
	    retval(i,j) = mat(i,j);
	    retval(i,j+3) = 0;
	    retval(i+3,j) = mat_dot(i,j);
	    retval(i+3,j+3) = mat(i,j);
	  }
	}
	return retval;
      }
void cvx_clustering ( double ** dist_mat, int fw_max_iter, int max_iter, int D, int N, double* lambda, double ** W) {
    // parameters
    double alpha = 0.1;
    double rho = 1;
    // iterative optimization
    double error = INF;
    double ** wone = mat_init (N, N);
    double ** wtwo = mat_init (N, N);
    double ** yone = mat_init (N, N);
    double ** ytwo = mat_init (N, N);
    double ** z = mat_init (N, N);
    double ** diffzero = mat_init (N, N);
    double ** diffone = mat_init (N, N);
    double ** difftwo = mat_init (N, N);
    mat_zeros (yone, N, N);
    mat_zeros (ytwo, N, N);
    mat_zeros (z, N, N);
    mat_zeros (diffzero, N, N);
    mat_zeros (diffone, N, N);
    mat_zeros (difftwo, N, N);

    int iter = 0; // Ian: usually we count up (instead of count down)
    while ( iter < max_iter ) { // stopping criteria

#ifdef SPARSE_CLUSTERING_DUMP
        cout << "it is place 0 iteration #" << iter << ", going to get into frank_wolfe"  << endl;
#endif
        // mat_set (wone, z, N, N);
        // mat_set (wtwo, z, N, N);
        // mat_zeros (wone, N, N);
        // mat_zeros (wtwo, N, N);
        // STEP ONE: resolve w_1 and w_2
        frank_wolf (dist_mat, yone, z, wone, rho, N, fw_max_iter); // for w_1
#ifdef SPARSE_CLUSTERING_DUMP
        cout << "norm2(w_1) = " << mat_norm2 (wone, N, N) << endl;
#endif

        blockwise_closed_form (ytwo, z, wtwo, rho, lambda, N);  // for w_2
#ifdef SPARSE_CLUSTERING_DUMP
        cout << "norm2(w_2) = " << mat_norm2 (wtwo, N, N) << endl;
#endif

        // STEP TWO: update z by averaging w_1 and w_2
        mat_add (wone, wtwo, z, N, N);
        mat_dot (0.5, z, z, N, N);
#ifdef SPARSE_CLUSTERING_DUMP
        cout << "norm2(z) = " << mat_norm2 (z, N, N) << endl;
#endif

        // STEP THREE: update the y_1 and y_2 by w_1, w_2 and z

        mat_sub (wone, z, diffone, N, N);
        double trace_wone_minus_z = mat_norm2 (diffone, N, N);
        mat_dot (alpha, diffone, diffone, N, N);
        mat_add (yone, diffone, yone, N, N);

        mat_sub (wtwo, z, difftwo, N, N);
        //double trace_wtwo_minus_z = mat_norm2 (difftwo, N, N);
        mat_dot (alpha, difftwo, difftwo, N, N);
        mat_add (ytwo, difftwo, ytwo, N, N);

        // STEP FOUR: trace the objective function
        if (iter % 100 == 0) {
            error = overall_objective (dist_mat, lambda, N, z);
            // get current number of employed centroid
#ifdef NCENTROID_DUMP
            int nCentroids = get_nCentroids (z, N, N);
#endif
            cout << "[Overall] iter = " << iter
                 << ", Overall Error: " << error
#ifdef NCENTROID_DUMP
                 << ", nCentroids: " << nCentroids
#endif
                 << endl;
        }
        iter ++;
    }

    // STEP FIVE: memory recollection
    mat_free (wone, N, N);
    mat_free (wtwo, N, N);
    mat_free (yone, N, N);
    mat_free (ytwo, N, N);
    mat_free (diffone, N, N);
    mat_free (difftwo, N, N);
    // STEP SIX: put converged solution to destination W
    mat_copy (z, W, N, N);
}
void blockwise_closed_form (double ** ytwo, double ** ztwo, double ** wtwo, double rho, double* lambda, int N) {

    // STEP ONE: compute the optimal solution for truncated problem
    double ** wbar = mat_init (N, N);
    mat_zeros (wbar, N, N);
    mat_dot (rho, ztwo, wbar, N, N); // wbar = rho * z_2
    mat_sub (wbar, ytwo, wbar, N, N); // wbar = rho * z_2 - y_2
    mat_dot (1.0/rho, wbar, wbar, N, N); // wbar = (rho * z_2 - y_2) / rho

    // STEP TWO: find the closed-form solution for second subproblem
    for (int j = 0; j < N; j ++) {
        // 1. bifurcate the set of values
        vector< pair<int,double> > alpha_vec;
        for (int i = 0; i < N; i ++) {
            double value = wbar[i][j];
            /*if( wbar[i][j] < 0 ){
              cerr << "wbar[" << i << "][" << j << "]" << endl;
              exit(0);
              }*/
            alpha_vec.push_back (make_pair(i, abs(value)));
        }
        // 2. sorting
        std::sort (alpha_vec.begin(), alpha_vec.end(), pairComparator);
        /*
           for (int i = 0; i < N; i ++) {
           if (alpha_vec[i].second != 0)
           cout << alpha_vec[i].second << endl;
           }
           */
        // 3. find mstar
        int mstar = 0; // number of elements support the sky
        double separator;
        double max_term = -INF, new_term;
        double sum_alpha = 0.0;
        for (int i = 0; i < N; i ++) {
            sum_alpha += alpha_vec[i].second;
            new_term = (sum_alpha - lambda[j]) / (i + 1.0);
            if ( new_term > max_term ) {
                separator = alpha_vec[i].second;
                max_term = new_term;
                mstar = i;
            }
        }
        // 4. assign closed-form solution to wtwo
        if( max_term < 0 ) {
            for(int i=0; i<N; i++)
                wtwo[i][j] = 0.0;
            continue;
        }
        for (int i = 0; i < N; i ++) {
            // harness vector of pair
            double value = wbar[i][j];
            if ( abs(value) >= separator ) {
                wtwo[i][j] = max_term;
            } else {
                // its ranking is above m*, directly inherit the wbar
                wtwo[i][j] = max(wbar[i][j],0.0);
            }
        }
    }
    // compute value of objective function
    double penalty = second_subproblem_obj (ytwo, ztwo, wtwo, rho, N, lambda);
    // report the #iter and objective function
    /*cout << "[Blockwise] second_subproblem_obj: " << penalty << endl;
      cout << endl;*/

    // STEP THREE: recollect temporary variable - wbar
    mat_free (wbar, N, N);
}