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