int main (int argc, char **argv)
{
/* ---------------------------------------------------------------------- */
/* get the file containing the input matrix */
/* ---------------------------------------------------------------------- */
FILE *ff = NULL ;
FILE *fb = NULL ;
if (argc <= 1)
{
printf("Usage is: cholmod_simple A.tri [B.txt (dense)]\n");
exit(0);
}
if (argc > 1)
ff = fopen(argv[1],"r");
if (argc > 2)
fb = fopen(argv[2], "r");
cholmod_sparse *A ;
cholmod_dense *x, *b, *r ;
cholmod_factor *L ;
double one [2] = {1,0}, m1 [2] = {-1,0} ; // basic scalars
cholmod_common c ;
cholmod_start (&c) ; /* start CHOLMOD */
A = cholmod_read_sparse (ff, &c) ; /* read in a matrix */
cholmod_print_sparse (A, (char *)"A", &c) ; /* print the matrix */
if (A->dtype) printf("A is float\n");
else printf("A is double\n");
if (A == NULL || A->stype == 0) /* A must be symmetric */
{
cholmod_free_sparse (&A, &c) ;
cholmod_finish (&c) ;
if (ff) fclose(ff);
if (fb) fclose(fb);
return (0) ;
}
if (fb)
b = cholmod_read_dense(fb, &c);
else
b = cholmod_ones (A->nrow, 1, A->xtype, &c) ; /* b = ones(n,1) */
double t0 = CPUTIME;
L = cholmod_analyze (A, &c) ; /* analyze */
cholmod_factorize (A, L, &c) ; /* factorize */
x = cholmod_solve (CHOLMOD_A, L, b, &c) ; /* solve Ax=b */
double t1 = CPUTIME;
if (c.dtype) printf("Compute is float\n");
else printf("Compute is double\n");
printf("Time: %12.4f \n", t1-t0);
r = cholmod_copy_dense (b, &c) ; /* r = b */
cholmod_sdmult (A, 0, m1, one, x, r, &c) ; /* r = r-Ax */
printf ("norm(b-Ax) %8.1e\n",
cholmod_norm_dense (r, 0, &c)) ; /* print norm(r) */
cholmod_free_factor (&L, &c) ; /* free matrices */
cholmod_free_sparse (&A, &c) ;
cholmod_free_dense (&r, &c) ;
cholmod_free_dense (&x, &c) ;
cholmod_free_dense (&b, &c) ;
cholmod_finish (&c) ; /* finish CHOLMOD */
return (0) ;
}
int main (void)
{
cholmod_sparse *A ;
cholmod_dense *x, *b, *r ;
cholmod_factor *L ;
double one [2] = {1,0}, m1 [2] = {-1,0} ; /* basic scalars */
cholmod_common c ;
cholmod_start (&c) ; /* start CHOLMOD */
A = cholmod_read_sparse (stdin, &c) ; /* read in a matrix */
cholmod_print_sparse (A, "A", &c) ; /* print the matrix */
if (A == NULL || A->stype == 0) /* A must be symmetric */
{
cholmod_free_sparse (&A, &c) ;
cholmod_finish (&c) ;
return (0) ;
}
b = cholmod_ones (A->nrow, 1, A->xtype, &c) ; /* b = ones(n,1) */
L = cholmod_analyze (A, &c) ; /* analyze */
cholmod_factorize (A, L, &c) ; /* factorize */
x = cholmod_solve (CHOLMOD_A, L, b, &c) ; /* solve Ax=b */
r = cholmod_copy_dense (b, &c) ; /* r = b */
cholmod_sdmult (A, 0, m1, one, x, r, &c) ; /* r = r-Ax */
printf ("norm(b-Ax) %8.1e\n",
cholmod_norm_dense (r, 0, &c)) ; /* print norm(r) */
cholmod_free_factor (&L, &c) ; /* free matrices */
cholmod_free_sparse (&A, &c) ;
cholmod_free_dense (&r, &c) ;
cholmod_free_dense (&x, &c) ;
cholmod_free_dense (&b, &c) ;
cholmod_finish (&c) ; /* finish CHOLMOD */
return (0) ;
}
cholmod_sparse *MultivariateFNormalSufficientSparse::compute_PTP() const
{
IMP_LOG(TERSE, "MVNsparse: computing PTP" << std::endl);
cholmod_sparse *eps = cholmod_dense_to_sparse(epsilon_, true, c_);
cholmod_sparse *tmp = cholmod_spsolve(CHOLMOD_A, L_, eps, c_);
cholmod_sparse *ptp = cholmod_aat(tmp, nullptr, 0, 1, c_);
cholmod_free_sparse(&eps, c_);
cholmod_free_sparse(&tmp, c_);
return ptp;
}
void get_ordering(cholmod_sparse* const NNE,
const int MIS_method,
const bool directed,
std::list<int>& ordering,
cholmod_common* const cholmod_c) {
cholmod_sparse* adja_mat;
switch(MIS_method) {
case FSTPOWORDER:
if (directed) {
// adja_mat = NNE | t(NNE)
cholmod_sparse* NNEt = cholmod_transpose(NNE, CHOLMOD_PATTERN, cholmod_c);
adja_mat = cholmod_add(NNE, NNEt, NULL, NULL, false, false, cholmod_c);
cholmod_free_sparse(&NNEt, cholmod_c);
} else {
// adja_mat = NNE
adja_mat = cholmod_copy_sparse(NNE, cholmod_c);
}
break;
case SNDPOWORDER:
case HEURISTIC:
adja_mat = get_second_power(NNE, directed, cholmod_c);
break;
default:
error("Unknown MIS method.");
}
// adja_mat_p: array with pointer to elements for each column
// where `i' is start and `i+1' is finish.
// Can thus be used to get col sums.
const int* const adja_mat_p = static_cast<const int*>(adja_mat->p);
std::list<std::pair<int,int> > tmp_order;
int n_vertices = static_cast<int>(NNE->ncol);
for (int i = 0; i < n_vertices; ++i) {
tmp_order.push_back(std::make_pair(adja_mat_p[i + 1] - adja_mat_p[i], i));
}
cholmod_free_sparse(&adja_mat, cholmod_c);
// sorts first according to colsum (i.e., first el)
// then by vertex id. As ID is unique, sorting is stable.
tmp_order.sort();
for (std::list<std::pair<int,int> >::const_iterator it = tmp_order.begin();
it != tmp_order.end(); ++it) {
ordering.push_back(it->second);
}
}
cholmod_sparse *MultivariateFNormalSufficientSparse::compute_PWP() const
{
// PWP = PW
IMP_LOG(TERSE, "MVNsparse: computing PWP" << std::endl);
//solve for X in Sigma*X=W
cholmod_sparse *tmp = cholmod_spsolve(CHOLMOD_A, L_, W_, c_);
//and then Y in trans(X)=Sigma*Y
cholmod_sparse *tx = cholmod_transpose(tmp, 1, c_);
cholmod_free_sparse(&tmp, c_);
cholmod_sparse *R = cholmod_spsolve(CHOLMOD_A, L_, tx, c_);
cholmod_free_sparse(&tx, c_);
IMP_LOG(TERSE, "MVNsparse: done" << std::endl);
return R;
}
/**
* "Indexing" aka subsetting : Compute x[i,j], also for vectors i and j
* Working via CHOLMOD_submatrix, see ./CHOLMOD/MatrixOps/cholmod_submatrix.c
* @param x CsparseMatrix
* @param i row indices (0-origin), or NULL (R's)
* @param j columns indices (0-origin), or NULL
*
* @return x[i,j] still CsparseMatrix --- currently, this loses dimnames
*/
SEXP Csparse_submatrix(SEXP x, SEXP i, SEXP j)
{
CHM_SP chx = AS_CHM_SP(x); /* << does diagU2N() when needed */
int rsize = (isNull(i)) ? -1 : LENGTH(i),
csize = (isNull(j)) ? -1 : LENGTH(j);
int Rkind = (chx->xtype != CHOLMOD_PATTERN) ? Real_kind(x) : 0;
R_CheckStack();
if (rsize >= 0 && !isInteger(i))
error(_("Index i must be NULL or integer"));
if (csize >= 0 && !isInteger(j))
error(_("Index j must be NULL or integer"));
if (!chx->stype) {/* non-symmetric Matrix */
return chm_sparse_to_SEXP(cholmod_submatrix(chx,
(rsize < 0) ? NULL : INTEGER(i), rsize,
(csize < 0) ? NULL : INTEGER(j), csize,
TRUE, TRUE, &c),
1, 0, Rkind, "",
/* FIXME: drops dimnames */ R_NilValue);
}
/* for now, cholmod_submatrix() only accepts "generalMatrix" */
CHM_SP tmp = cholmod_copy(chx, /* stype: */ 0, chx->xtype, &c);
CHM_SP ans = cholmod_submatrix(tmp,
(rsize < 0) ? NULL : INTEGER(i), rsize,
(csize < 0) ? NULL : INTEGER(j), csize,
TRUE, TRUE, &c);
cholmod_free_sparse(&tmp, &c);
return chm_sparse_to_SEXP(ans, 1, 0, Rkind, "", R_NilValue);
}
SEXP dsCMatrix_chol(SEXP x, SEXP pivot)
{
cholmod_factor
*N = as_cholmod_factor(dsCMatrix_Cholesky(x, pivot,
ScalarLogical(FALSE),
ScalarLogical(FALSE)));
/* Must use a copy; cholmod_factor_to_sparse modifies first arg. */
cholmod_factor *Ncp = cholmod_copy_factor(N, &c);
cholmod_sparse *L, *R;
SEXP ans;
L = cholmod_factor_to_sparse(Ncp, &c); cholmod_free_factor(&Ncp, &c);
R = cholmod_transpose(L, /*values*/ 1, &c); cholmod_free_sparse(&L, &c);
ans = PROTECT(chm_sparse_to_SEXP(R, /*cholmod_free*/ 1,
/*uploT*/ 1, /*diag*/ "N",
GET_SLOT(x, Matrix_DimNamesSym)));
if (asLogical(pivot)) {
SEXP piv = PROTECT(allocVector(INTSXP, N->n));
int *dest = INTEGER(piv), *src = (int*)N->Perm, i;
for (i = 0; i < N->n; i++) dest[i] = src[i] + 1;
setAttrib(ans, install("pivot"), piv);
/* FIXME: Because of the cholmod_factor -> S4 obj ->
* cholmod_factor conversions, the value of N->minor will
* always be N->n. Change as_cholmod_factor and
* chm_factor_as_SEXP to keep track of Minor.
*/
setAttrib(ans, install("rank"), ScalarInteger((size_t) N->minor));
UNPROTECT(1);
}
Free(N);
UNPROTECT(1);
return ans;
}
void MultivariateFNormalSufficientSparse::set_W(const SparseMatrix<double>& W)
{
if (W_) cholmod_free_sparse(&W_, c_);
cholmod_sparse Wtmp = Eigen::viewAsCholmod(
W.selfadjointView<Eigen::Upper>());
//W_ = cholmod_copy_sparse(&Wtmp, c_);
W_ = cholmod_copy(&Wtmp, 0, 1, c_); //unsym for spsolve
}
cholmod_sparse *
MultivariateFNormalSufficientSparse::evaluate_derivative_Sigma() const
{
//d(-log(p))/dSigma = 1/2 (N P - N P epsilon transpose(epsilon) P - P W P)
IMP_LOG(TERSE, "MVNsparse: evaluate_derivative_Sigma() = " << std::endl);
cholmod_sparse *ptp(compute_PTP());
cholmod_sparse *pwp(compute_PWP());
//std::cout << " ptp " << std::endl << ptp << std::endl << std::endl;
//std::cout << " pwp " << std::endl << pwp << std::endl << std::endl;
static double one[2]={1,0};
static double minusone[2]={-1,0};
cholmod_sparse *tmp =
cholmod_add(P_, ptp, one, minusone, true, false, c_);
double enn[2]={0.5*N_,0};
static double ptfive[2]={-0.5,0};
cholmod_sparse *R = cholmod_add(tmp, pwp, enn, ptfive, true, false, c_);
cholmod_free_sparse(&ptp, c_);
cholmod_free_sparse(&pwp, c_);
cholmod_free_sparse(&tmp, c_);
return R;
}
/* FIXME: Create a more general version of this operation */
SEXP dsCMatrix_to_dgTMatrix(SEXP x)
{
cholmod_sparse *A = as_cholmod_sparse(x);
cholmod_sparse *Afull = cholmod_copy(A, /*stype*/ 0, /*mode*/ 1, &c);
cholmod_triplet *At = cholmod_sparse_to_triplet(Afull, &c);
if (!A->stype)
error("Non-symmetric matrix passed to dsCMatrix_to_dgTMatrix");
Free(A); cholmod_free_sparse(&Afull, &c);
return chm_triplet_to_SEXP(At, 1, /*uploT*/ 0, "",
GET_SLOT(x, Matrix_DimNamesSym));
}
/**
* Return a SuiteSparse QR factorization of the sparse matrix A
*
* @param Ap (pointer to) a [m x n] dgCMatrix
* @param ordering integer SEXP specifying the ordering strategy to be used
* see SPQR/Include/SuiteSparseQR_definitions.h
* @param econ integer SEXP ("economy"): number of rows of R and columns of Q
* to return. The default is m. Using n gives the standard economy form.
* A value less than the estimated rank r is set to r, so econ=0 gives the
* "rank-sized" factorization, where nrow(R)==nnz(diag(R))==r.
* @param tol double SEXP: if tol <= -2 use SPQR's default,
* if -2 < tol < 0, then no tol is used; otherwise,
* tol > 0, use as tolerance: columns with 2-norm <= tol treated as 0
*
*
* @return SEXP "SPQR" object with slots (Q, R, p, rank, Dim):
* Q: dgCMatrix; R: dgCMatrix [subject to change to dtCMatrix FIXME ?]
* p: integer: 0-based permutation (or length 0 <=> identity);
* rank: integer, the "revealed" rank Dim: integer, original matrix dim.
*/
SEXP dgCMatrix_SPQR(SEXP Ap, SEXP ordering, SEXP econ, SEXP tol)
{
/* SEXP ans = PROTECT(allocVector(VECSXP, 4)); */
SEXP ans = PROTECT(NEW_OBJECT(MAKE_CLASS("SPQR")));
CHM_SP A = AS_CHM_SP(Ap), Q, R;
SuiteSparse_long *E, rank;/* not always = int FIXME (Windows_64 ?) */
if ((rank = SuiteSparseQR_C_QR(asInteger(ordering),
asReal(tol),/* originally had SPQR_DEFAULT_TOL */
(SuiteSparse_long)asInteger(econ),/* originally had 0 */
A, &Q, &R, &E, &cl)) == -1)
error(_("SuiteSparseQR_C_QR returned an error code"));
slot_dup(ans, Ap, Matrix_DimSym);
/* SET_VECTOR_ELT(ans, 0, */
/* chm_sparse_to_SEXP(Q, 0, 0, 0, "", R_NilValue)); */
SET_SLOT(ans, install("Q"),
chm_sparse_to_SEXP(Q, 0, 0, 0, "", R_NilValue));
/* Also gives a dgCMatrix (not a dtC* *triangular*) :
* may make sense if to be used in the "spqr_solve" routines .. ?? */
/* SET_VECTOR_ELT(ans, 1, */
/* chm_sparse_to_SEXP(R, 0, 0, 0, "", R_NilValue)); */
SET_SLOT(ans, install("R"),
chm_sparse_to_SEXP(R, 0, 0, 0, "", R_NilValue));
cholmod_free_sparse(&Al, &cl);
cholmod_free_sparse(&R, &cl);
cholmod_free_sparse(&Q, &cl);
if (E) {
int *Er;
SET_VECTOR_ELT(ans, 2, allocVector(INTSXP, A->ncol));
Er = INTEGER(VECTOR_ELT(ans, 2));
for (int i = 0; i < A->ncol; i++) Er[i] = (int) E[i];
Free(E);
} else SET_VECTOR_ELT(ans, 2, allocVector(INTSXP, 0));
SET_VECTOR_ELT(ans, 3, ScalarInteger((int)rank));
UNPROTECT(1);
return ans;
}
/* Computes x'x or x x' -- *also* for Tsparse (triplet = TRUE)
see Csparse_Csparse_crossprod above for x'y and x y' */
SEXP Csparse_crossprod(SEXP x, SEXP trans, SEXP triplet)
{
int trip = asLogical(triplet),
tr = asLogical(trans); /* gets reversed because _aat is tcrossprod */
#ifdef AS_CHM_DIAGU2N_FIXED_FINALLY
CHM_TR cht = trip ? AS_CHM_TR(x) : (CHM_TR) NULL;
#else /* workaround needed:*/
SEXP xx = PROTECT(Tsparse_diagU2N(x));
CHM_TR cht = trip ? AS_CHM_TR__(xx) : (CHM_TR) NULL;
#endif
CHM_SP chcp, chxt,
chx = (trip ?
cholmod_triplet_to_sparse(cht, cht->nnz, &c) :
AS_CHM_SP(x));
SEXP dn = PROTECT(allocVector(VECSXP, 2));
R_CheckStack();
if (!tr) chxt = cholmod_transpose(chx, chx->xtype, &c);
chcp = cholmod_aat((!tr) ? chxt : chx, (int *) NULL, 0, chx->xtype, &c);
if(!chcp) {
UNPROTECT(1);
error(_("Csparse_crossprod(): error return from cholmod_aat()"));
}
cholmod_band_inplace(0, chcp->ncol, chcp->xtype, chcp, &c);
chcp->stype = 1;
if (trip) cholmod_free_sparse(&chx, &c);
if (!tr) cholmod_free_sparse(&chxt, &c);
SET_VECTOR_ELT(dn, 0, /* establish dimnames */
duplicate(VECTOR_ELT(GET_SLOT(x, Matrix_DimNamesSym),
(tr) ? 0 : 1)));
SET_VECTOR_ELT(dn, 1, duplicate(VECTOR_ELT(dn, 0)));
#ifdef AS_CHM_DIAGU2N_FIXED_FINALLY
UNPROTECT(1);
#else
UNPROTECT(2);
#endif
return chm_sparse_to_SEXP(chcp, 1, 0, 0, "", dn);
}
cholmod_sparse* get_second_power(cholmod_sparse* const NNE,
const bool directed,
cholmod_common* const cholmod_c) {
cholmod_sparse* out;
if (directed) {
// out = (NNE | t(NNE) | t(NNE) %*% NNE) & !I
cholmod_sparse* NNEt = cholmod_transpose(NNE, CHOLMOD_PATTERN, cholmod_c);
cholmod_sparse* NNEtNNE = cholmod_aat(NNEt, NULL, 0, -1, cholmod_c); // -1 = no diagnol
cholmod_sparse* tmp = cholmod_add(NNE, NNEt, NULL, NULL, false, false, cholmod_c);
cholmod_free_sparse(&NNEt, cholmod_c);
out = cholmod_add(tmp, NNEtNNE, NULL, NULL, false, false, cholmod_c);
cholmod_free_sparse(&NNEtNNE, cholmod_c);
cholmod_free_sparse(&tmp, cholmod_c);
} else {
// out = (NNE | t(NNE) %*% NNE) & !I
cholmod_sparse* NNEtNNE = cholmod_aat(NNE, NULL, 0, -1, cholmod_c); // -1 = no diagnol
out = cholmod_add(NNE, NNEtNNE, NULL, NULL, false, false, cholmod_c);
cholmod_free_sparse(&NNEtNNE, cholmod_c);
}
return out;
}
double GaussianProcessInterpolationRestraintSparse::unprotected_evaluate(
DerivativeAccumulator *accum) const
{
//check if the functions have changed
const_cast<GaussianProcessInterpolationRestraintSparse*>(this)->
update_mean_and_covariance();
double ene = mvn_->evaluate();
if (accum)
{
cholmod_dense *dmv = mvn_->evaluate_derivative_FM();
if (dmv->xtype != CHOLMOD_REAL)
IMP_THROW("matrix type is not real, update code here first",
ModelException);
double *dmvx=(double*) dmv->x;
//derivatives for mean particles
for (size_t i=0; i<M_; i++)
{
DerivativeAccumulator a(*accum, dmvx[i]);
gpi_->mean_function_->add_to_derivatives(gpi_->x_[i], a);
}
cholmod_free_dense(&dmv, c_);
//derivatives for covariance particles
cholmod_sparse *tmp(mvn_->evaluate_derivative_Sigma());
cholmod_triplet *dmvS = cholmod_sparse_to_triplet(tmp, c_);
cholmod_free_sparse(&tmp, c_);
if ((dmvS->itype != CHOLMOD_INT) && (dmvS->xtype != CHOLMOD_REAL))
IMP_THROW("matrix type is not real or coefficients are not int!",
ModelException);
int *dmvi=(int*) dmvS->i;
int *dmvj=(int*) dmvS->j;
dmvx=(double*) dmvS->x;
for (size_t p=0; p<dmvS->nzmax; ++p)
{
int i=dmvi[p];
int j=dmvj[p];
double val=dmvx[p];
DerivativeAccumulator a(*accum, val);
gpi_->covariance_function_->add_to_derivatives(
gpi_->x_[i],gpi_->x_[j], a);
}
cholmod_free_triplet(&dmvS,c_);
}
return ene;
}
double MultivariateFNormalSufficientSparse::trace_WP() const
{
//solve for Sigma.X=W
//cholmod_print_sparse(PW_,"PW",c_);
//isolate diagonal
cholmod_sparse *tmp = cholmod_band(PW_, 0, 0, 1, c_);
//cholmod_print_sparse(tmp,"diag(PW)",c_);
double trace=0;
if ((tmp->itype != CHOLMOD_INT) || (tmp->xtype != CHOLMOD_REAL))
IMP_THROW("matrix types different from int and double",
ModelException);
double *x = (double *) tmp->x;
for (size_t i=0; i < tmp->nzmax; ++i) trace += x[i];
cholmod_free_sparse(&tmp, c_);
IMP_LOG(TERSE, "MVNsparse: trace(WP) = " << trace << std::endl);
return trace;
}
void findMaxIS_in_sp(cholmod_sparse* const NNE,
const bool directed,
std::list<int>& MaxIs,
cholmod_common* const cholmod_c) {
cholmod_sparse* second_power = get_second_power(NNE, directed, cholmod_c);
const int* const sp_p = static_cast<const int*>(second_power->p);
const int* const sp_i = static_cast<const int*>(second_power->i);
bool inSetI[NNE->ncol];
std::fill_n(inSetI, NNE->ncol, true);
int removed_indices[NNE->ncol];
int sizeMaxIS = 0;
recur_MaxIS(inSetI, 0, removed_indices,
removed_indices + NNE->ncol,
sp_p, sp_i, 0, &sizeMaxIS, MaxIs);
cholmod_free_sparse(&second_power, cholmod_c);
}
// doing cholesky decomposition
void Algebra::CK_decomp(Matrix &A, cholmod_factor *&L, cholmod_common *cm, size_t &peak_mem, size_t & CK_mem){
// doing factorization first
cholmod_triplet * T;
size_t n_row = A.get_row();
size_t n_col = A.get_row();
size_t nnz = A.size();
int *Ti;
int *Tj;
double *Tx;
int stype = -1;// lower triangular storage
T = cholmod_allocate_triplet(n_row, n_col, nnz, stype,
CHOLMOD_REAL, cm);
Ti = static_cast<int *>(T->i);
Tj = static_cast<int *>(T->j);
Tx = static_cast<double *>(T->x);
// copy data into T
for(size_t k=0;k<nnz;k++){
Ti[k] = A.Ti[k];
Tj[k] = A.Tj[k];
Tx[k] = A.Tx[k];
}
T->nnz = nnz;
A.Ti.clear();
A.Tj.clear();
A.Tx.clear();
cholmod_sparse * A_cholmod;
A_cholmod = cholmod_triplet_to_sparse(T, nnz, cm);
// free the triplet pointer
cholmod_free_triplet(&T, cm);
//cm->supernodal = -1;
L = cholmod_analyze(A_cholmod, cm);
//L->ordering = CHOLMOD_NATURAL;
cholmod_factorize(A_cholmod, L, cm);
//cholmod_print_factor(L, "L", cm);
//if(peak_mem < cm->memory_usage)
//peak_mem = cm->memory_usage;
//CK_mem += cm->lnz;
cholmod_free_sparse(&A_cholmod, cm);
}
SEXP Csparse_diagU2N(SEXP x)
{
const char *cl = class_P(x);
/* dtCMatrix, etc; [1] = the second character =?= 't' for triangular */
if (cl[1] != 't' || *diag_P(x) != 'U') {
/* "trivially fast" when not triangular (<==> no 'diag' slot),
or not *unit* triangular */
return (x);
}
else { /* unit triangular (diag='U'): "fill the diagonal" & diag:= "N" */
CHM_SP chx = AS_CHM_SP__(x);
CHM_SP eye = cholmod_speye(chx->nrow, chx->ncol, chx->xtype, &c);
double one[] = {1, 0};
CHM_SP ans = cholmod_add(chx, eye, one, one, TRUE, TRUE, &c);
int uploT = (*uplo_P(x) == 'U') ? 1 : -1;
int Rkind = (chx->xtype != CHOLMOD_PATTERN) ? Real_kind(x) : 0;
R_CheckStack();
cholmod_free_sparse(&eye, &c);
return chm_sparse_to_SEXP(ans, 1, uploT, Rkind, "N",
GET_SLOT(x, Matrix_DimNamesSym));
}
}
SEXP Csparse_Csparse_crossprod(SEXP a, SEXP b, SEXP trans)
{
int tr = asLogical(trans);
CHM_SP
cha = AS_CHM_SP(a),
chb = AS_CHM_SP(b),
chTr, chc;
const char *cl_a = class_P(a), *cl_b = class_P(b);
char diag[] = {'\0', '\0'};
int uploT = 0;
SEXP dn = PROTECT(allocVector(VECSXP, 2));
R_CheckStack();
chTr = cholmod_transpose((tr) ? chb : cha, chb->xtype, &c);
chc = cholmod_ssmult((tr) ? cha : chTr, (tr) ? chTr : chb,
/*out_stype:*/ 0, cha->xtype, /*out sorted:*/ 1, &c);
cholmod_free_sparse(&chTr, &c);
/* Preserve triangularity and unit-triangularity if appropriate;
* see Csparse_Csparse_prod() for comments */
if (cl_a[1] == 't' && cl_b[1] == 't')
if(*uplo_P(a) != *uplo_P(b)) { /* one 'U', the other 'L' */
uploT = (*uplo_P(b) == 'U') ? 1 : -1;
if(*diag_P(a) == 'U' && *diag_P(b) == 'U') { /* return UNIT-triag. */
chm_diagN2U(chc, uploT, /* do_realloc */ FALSE);
diag[0]= 'U';
}
else diag[0]= 'N';
}
SET_VECTOR_ELT(dn, 0, /* establish dimnames */
duplicate(VECTOR_ELT(GET_SLOT(a, Matrix_DimNamesSym), (tr) ? 0 : 1)));
SET_VECTOR_ELT(dn, 1,
duplicate(VECTOR_ELT(GET_SLOT(b, Matrix_DimNamesSym), (tr) ? 0 : 1)));
UNPROTECT(1);
return chm_sparse_to_SEXP(chc, 1, uploT, /*Rkind*/0, diag, dn);
}
SEXP gCMatrix_colSums(SEXP x, SEXP NArm, SEXP spRes, SEXP trans, SEXP means)
{
int mn = asLogical(means), sp = asLogical(spRes), tr = asLogical(trans);
/* cholmod_sparse: drawback of coercing lgC to double: */
CHM_SP cx = AS_CHM_SP(x);
R_CheckStack();
if (tr) {
cholmod_sparse *cxt = cholmod_transpose(cx, (int)cx->xtype, &c);
cx = cxt;
}
/* everything else *after* the above potential transpose : */
/* Don't declarations here require the C99 standard? Can we assume C99? */
int j, nc = cx->ncol;
int *xp = (int *)(cx -> p);
#ifdef _has_x_slot_
int na_rm = asLogical(NArm), i, dnm = 0/*Wall*/;
double *xx = (double *)(cx -> x);
#endif
SEXP ans = PROTECT(sp ? NEW_OBJECT(MAKE_CLASS(SparseResult_class))
: allocVector(SXP_ans, nc));
if (sp) { /* sparseResult - never allocating length-nc ... */
int nza, i1, i2, p, *ai;
Type_ans *ax;
for (j = 0, nza = 0; j < nc; j++)
if(xp[j] < xp[j + 1])
nza++;
ai = INTEGER(ALLOC_SLOT(ans, Matrix_iSym, INTSXP, nza));
ax = STYP_ans(ALLOC_SLOT(ans, Matrix_xSym, SXP_ans, nza));
SET_SLOT(ans, Matrix_lengthSym, ScalarInteger(nc));
i2 = xp[0];
for (j = 1, p = 0; j <= nc; j++) {
/* j' =j+1, since 'i' slot will be 1-based */
i1 = i2; i2 = xp[j];
if(i1 < i2) {
Type_ans sum;
ColSUM_column(i1,i2, sum);
ai[p] = j;
ax[p++] = sum;
}
}
}
else { /* "numeric" (non sparse) result */
Type_ans *a = STYP_ans(ans);
for (j = 0; j < nc; j++) {
ColSUM_column(xp[j], xp[j + 1], a[j]);
}
}
if (tr) cholmod_free_sparse(&cx, &c);
UNPROTECT(1);
return ans;
}
int main (int argc, char **argv)
{
double resid [4], t, ta, tf, ts [3], tot, bnorm, xnorm, anorm, rnorm, fl,
anz, axbnorm, rnorm2, resid2, rcond ;
FILE *f ;
cholmod_sparse *A ;
cholmod_dense *X = NULL, *B, *W, *R ;
double one [2], zero [2], minusone [2], beta [2], xlnz ;
cholmod_common Common, *cm ;
cholmod_factor *L ;
double *Bx, *Rx, *Xx ;
int i, n, isize, xsize, ordering, xtype, s, ss, lnz ;
int trial, method, L_is_super ;
int ver [3] ;
ts[0] = 0.;
ts[1] = 0.;
ts[2] = 0.;
/* ---------------------------------------------------------------------- */
/* get the file containing the input matrix */
/* ---------------------------------------------------------------------- */
ff = NULL ;
if (argc > 1)
{
if ((f = fopen (argv [1], "r")) == NULL)
{
my_handler (CHOLMOD_INVALID, __FILE__, __LINE__,
"unable to open file") ;
}
ff = f ;
}
else
{
f = stdin ;
}
/* ---------------------------------------------------------------------- */
/* start CHOLMOD and set parameters */
/* ---------------------------------------------------------------------- */
cm = &Common ;
cholmod_start (cm) ;
CHOLMOD_FUNCTION_DEFAULTS (cm) ; /* just for testing (not required) */
/* use default parameter settings, except for the error handler. This
* demo program terminates if an error occurs (out of memory, not positive
* definite, ...). It makes the demo program simpler (no need to check
* CHOLMOD error conditions). This non-default parameter setting has no
* effect on performance. */
cm->error_handler = my_handler ;
/* Note that CHOLMOD will do a supernodal LL' or a simplicial LDL' by
* default, automatically selecting the latter if flop/nnz(L) < 40. */
/* ---------------------------------------------------------------------- */
/* create basic scalars */
/* ---------------------------------------------------------------------- */
zero [0] = 0 ;
zero [1] = 0 ;
one [0] = 1 ;
one [1] = 0 ;
minusone [0] = -1 ;
minusone [1] = 0 ;
beta [0] = 1e-6 ;
beta [1] = 0 ;
/* ---------------------------------------------------------------------- */
/* read in a matrix */
/* ---------------------------------------------------------------------- */
printf ("\n---------------------------------- cholmod_demo:\n") ;
cholmod_version (ver) ;
printf ("cholmod version %d.%d.%d\n", ver [0], ver [1], ver [2]) ;
SuiteSparse_version (ver) ;
printf ("SuiteSparse version %d.%d.%d\n", ver [0], ver [1], ver [2]) ;
A = cholmod_read_sparse (f, cm) ;
if (ff != NULL)
{
fclose (ff) ;
ff = NULL ;
}
anorm = cholmod_norm_sparse (A, 0, cm) ;
xtype = A->xtype ;
printf ("norm (A,inf) = %g\n", anorm) ;
printf ("norm (A,1) = %g\n", cholmod_norm_sparse (A, 1, cm)) ;
cholmod_print_sparse (A, "A", cm) ;
if (A->nrow > A->ncol)
{
/* Transpose A so that A'A+beta*I will be factorized instead */
cholmod_sparse *C = cholmod_transpose (A, 2, cm) ;
cholmod_free_sparse (&A, cm) ;
A = C ;
printf ("transposing input matrix\n") ;
}
/* ---------------------------------------------------------------------- */
/* create an arbitrary right-hand-side */
/* ---------------------------------------------------------------------- */
n = A->nrow ;
B = cholmod_zeros (n, 1, xtype, cm) ;
Bx = B->x ;
#if GHS
{
/* b = A*ones(n,1), used by Gould, Hu, and Scott in their experiments */
cholmod_dense *X0 ;
X0 = cholmod_ones (A->ncol, 1, xtype, cm) ;
cholmod_sdmult (A, 0, one, zero, X0, B, cm) ;
cholmod_free_dense (&X0, cm) ;
}
#else
if (xtype == CHOLMOD_REAL)
{
/* real case */
for (i = 0 ; i < n ; i++)
{
double x = n ;
Bx [i] = 1 + i / x ;
}
}
else
{
/* complex case */
for (i = 0 ; i < n ; i++)
{
double x = n ;
Bx [2*i ] = 1 + i / x ; /* real part of B(i) */
Bx [2*i+1] = (x/2 - i) / (3*x) ; /* imag part of B(i) */
}
}
#endif
cholmod_print_dense (B, "B", cm) ;
bnorm = cholmod_norm_dense (B, 0, cm) ; /* max norm */
printf ("bnorm %g\n", bnorm) ;
/* ---------------------------------------------------------------------- */
/* analyze and factorize */
/* ---------------------------------------------------------------------- */
t = CPUTIME ;
L = cholmod_analyze (A, cm) ;
ta = CPUTIME - t ;
ta = MAX (ta, 0) ;
printf ("Analyze: flop %g lnz %g\n", cm->fl, cm->lnz) ;
if (A->stype == 0)
{
printf ("Factorizing A*A'+beta*I\n") ;
t = CPUTIME ;
cholmod_factorize_p (A, beta, NULL, 0, L, cm) ;
tf = CPUTIME - t ;
tf = MAX (tf, 0) ;
}
else
{
printf ("Factorizing A\n") ;
t = CPUTIME ;
cholmod_factorize (A, L, cm) ;
tf = CPUTIME - t ;
tf = MAX (tf, 0) ;
}
cholmod_print_factor (L, "L", cm) ;
/* determine the # of integers's and reals's in L. See cholmod_free */
if (L->is_super)
{
s = L->nsuper + 1 ;
xsize = L->xsize ;
ss = L->ssize ;
isize =
n /* L->Perm */
+ n /* L->ColCount, nz in each column of 'pure' L */
+ s /* L->pi, column pointers for L->s */
+ s /* L->px, column pointers for L->x */
+ s /* L->super, starting column index of each supernode */
+ ss ; /* L->s, the pattern of the supernodes */
}
else
{
/* this space can increase if you change parameters to their non-
* default values (cm->final_pack, for example). */
lnz = L->nzmax ;
xsize = lnz ;
isize =
n /* L->Perm */
+ n /* L->ColCount, nz in each column of 'pure' L */
+ n+1 /* L->p, column pointers */
+ lnz /* L->i, integer row indices */
+ n /* L->nz, nz in each column of L */
+ n+2 /* L->next, link list */
+ n+2 ; /* L->prev, link list */
}
/* solve with Bset will change L from simplicial to supernodal */
rcond = cholmod_rcond (L, cm) ;
L_is_super = L->is_super ;
/* ---------------------------------------------------------------------- */
/* solve */
/* ---------------------------------------------------------------------- */
for (method = 0 ; method <= 3 ; method++)
{
double x = n ;
if (method == 0)
{
/* basic solve, just once */
t = CPUTIME ;
X = cholmod_solve (CHOLMOD_A, L, B, cm) ;
ts [0] = CPUTIME - t ;
ts [0] = MAX (ts [0], 0) ;
}
else if (method == 1)
{
/* basic solve, many times, but keep the last one */
t = CPUTIME ;
for (trial = 0 ; trial < NTRIALS ; trial++)
{
cholmod_free_dense (&X, cm) ;
Bx [0] = 1 + trial / x ; /* tweak B each iteration */
X = cholmod_solve (CHOLMOD_A, L, B, cm) ;
}
ts [1] = CPUTIME - t ;
ts [1] = MAX (ts [1], 0) / NTRIALS ;
}
else if (method == 2)
{
/* solve with reused workspace */
cholmod_dense *Ywork = NULL, *Ework = NULL ;
cholmod_free_dense (&X, cm) ;
t = CPUTIME ;
for (trial = 0 ; trial < NTRIALS ; trial++)
{
Bx [0] = 1 + trial / x ; /* tweak B each iteration */
cholmod_solve2 (CHOLMOD_A, L, B, NULL, &X, NULL,
&Ywork, &Ework, cm) ;
}
cholmod_free_dense (&Ywork, cm) ;
cholmod_free_dense (&Ework, cm) ;
ts [2] = CPUTIME - t ;
ts [2] = MAX (ts [2], 0) / NTRIALS ;
}
else
{
/* solve with reused workspace and sparse Bset */
cholmod_dense *Ywork = NULL, *Ework = NULL ;
cholmod_dense *X2 = NULL, *B2 = NULL ;
cholmod_sparse *Bset, *Xset = NULL ;
int *Bsetp, *Bseti, *Xsetp, *Xseti, xlen, j, k, *Lnz ;
double *X1x, *X2x, *B2x, err ;
FILE *timelog = fopen ("timelog.m", "w") ;
if (timelog) fprintf (timelog, "results = [\n") ;
B2 = cholmod_zeros (n, 1, xtype, cm) ;
B2x = B2->x ;
Bset = cholmod_allocate_sparse (n, 1, 1, FALSE, TRUE, 0,
CHOLMOD_PATTERN, cm) ;
Bsetp = Bset->p ;
Bseti = Bset->i ;
Bsetp [0] = 0 ; /* nnz(B) is 1 (it can be anything) */
Bsetp [1] = 1 ;
resid [3] = 0 ;
for (i = 0 ; i < MIN (100,n) ; i++)
{
/* B (i) is nonzero, all other entries are ignored
(implied to be zero) */
Bseti [0] = i ;
if (xtype == CHOLMOD_REAL)
{
B2x [i] = 3.1 * i + 0.9 ;
}
else
{
B2x [2*i ] = i + 0.042 ;
B2x [2*i+1] = i - 92.7 ;
}
/* first get the entire solution, to compare against */
cholmod_solve2 (CHOLMOD_A, L, B2, NULL, &X, NULL,
&Ywork, &Ework, cm) ;
/* now get the sparse solutions; this will change L from
supernodal to simplicial */
if (i == 0)
{
/* first solve can be slower because it has to allocate
space for X2, Xset, etc, and change L.
So don't time it */
cholmod_solve2 (CHOLMOD_A, L, B2, Bset, &X2, &Xset,
&Ywork, &Ework, cm) ;
}
t = CPUTIME ;
for (trial = 0 ; trial < NTRIALS ; trial++)
{
/* solve Ax=b but only to get x(i).
b is all zero except for b(i).
This takes O(xlen) time */
cholmod_solve2 (CHOLMOD_A, L, B2, Bset, &X2, &Xset,
&Ywork, &Ework, cm) ;
}
t = CPUTIME - t ;
t = MAX (t, 0) / NTRIALS ;
/* check the solution and log the time */
Xsetp = Xset->p ;
Xseti = Xset->i ;
xlen = Xsetp [1] ;
X1x = X->x ;
X2x = X2->x ;
Lnz = L->nz ;
/*
printf ("\ni %d xlen %d (%p %p)\n", i, xlen, X1x, X2x) ;
*/
if (xtype == CHOLMOD_REAL)
{
fl = 2 * xlen ;
for (k = 0 ; k < xlen ; k++)
{
j = Xseti [k] ;
fl += 4 * Lnz [j] ;
err = X1x [j] - X2x [j] ;
err = ABS (err) ;
resid [3] = MAX (resid [3], err) ;
}
}
else
{
fl = 16 * xlen ;
for (k = 0 ; k < xlen ; k++)
{
j = Xseti [k] ;
fl += 16 * Lnz [j] ;
err = X1x [2*j ] - X2x [2*j ] ;
err = ABS (err) ;
resid [3] = MAX (resid [3], err) ;
err = X1x [2*j+1] - X2x [2*j+1] ;
err = ABS (err) ;
resid [3] = MAX (resid [3], err) ;
}
}
if (timelog) fprintf (timelog, "%g %g %g %g\n",
(double) i, (double) xlen, fl, t);
/* clear B for the next test */
if (xtype == CHOLMOD_REAL)
{
B2x [i] = 0 ;
}
else
{
B2x [2*i ] = 0 ;
B2x [2*i+1] = 0 ;
}
}
if (timelog)
{
fprintf (timelog, "] ; resid = %g ;\n", resid [3]) ;
fprintf (timelog, "lnz = %g ;\n", cm->lnz) ;
fprintf (timelog, "t = %g ; %% dense solve time\n", ts [2]) ;
fclose (timelog) ;
}
resid [3] = resid [3] / cholmod_norm_dense (X, 1, cm) ;
cholmod_free_dense (&Ywork, cm) ;
cholmod_free_dense (&Ework, cm) ;
cholmod_free_dense (&X2, cm) ;
cholmod_free_dense (&B2, cm) ;
cholmod_free_sparse (&Xset, cm) ;
cholmod_free_sparse (&Bset, cm) ;
}
/* ------------------------------------------------------------------ */
/* compute the residual */
/* ------------------------------------------------------------------ */
if (method < 3)
{
if (A->stype == 0)
{
/* (AA'+beta*I)x=b is the linear system that was solved */
/* W = A'*X */
W = cholmod_allocate_dense (A->ncol, 1, A->ncol, xtype, cm) ;
cholmod_sdmult (A, 2, one, zero, X, W, cm) ;
/* R = B - beta*X */
R = cholmod_zeros (n, 1, xtype, cm) ;
Rx = R->x ;
Xx = X->x ;
if (xtype == CHOLMOD_REAL)
{
for (i = 0 ; i < n ; i++)
{
Rx [i] = Bx [i] - beta [0] * Xx [i] ;
}
}
else
{
/* complex case */
for (i = 0 ; i < n ; i++)
{
Rx [2*i ] = Bx [2*i ] - beta [0] * Xx [2*i ] ;
Rx [2*i+1] = Bx [2*i+1] - beta [0] * Xx [2*i+1] ;
}
}
/* R = A*W - R */
cholmod_sdmult (A, 0, one, minusone, W, R, cm) ;
cholmod_free_dense (&W, cm) ;
}
else
{
/* Ax=b was factorized and solved, R = B-A*X */
R = cholmod_copy_dense (B, cm) ;
cholmod_sdmult (A, 0, minusone, one, X, R, cm) ;
}
rnorm = cholmod_norm_dense (R, 0, cm) ; /* max abs. entry */
xnorm = cholmod_norm_dense (X, 0, cm) ; /* max abs. entry */
axbnorm = (anorm * xnorm + bnorm + ((n == 0) ? 1 : 0)) ;
resid [method] = rnorm / axbnorm ;
}
}
tot = ta + tf + ts [0] ;
/* ---------------------------------------------------------------------- */
/* iterative refinement (real symmetric case only) */
/* ---------------------------------------------------------------------- */
resid2 = -1 ;
if (A->stype != 0 && A->xtype == CHOLMOD_REAL)
{
cholmod_dense *R2 ;
/* R2 = A\(B-A*X) */
R2 = cholmod_solve (CHOLMOD_A, L, R, cm) ;
/* compute X = X + A\(B-A*X) */
Xx = X->x ;
Rx = R2->x ;
for (i = 0 ; i < n ; i++)
{
Xx [i] = Xx [i] + Rx [i] ;
}
cholmod_free_dense (&R2, cm) ;
cholmod_free_dense (&R, cm) ;
/* compute the new residual, R = B-A*X */
R = cholmod_copy_dense (B, cm) ;
cholmod_sdmult (A, 0, minusone, one, X, R, cm) ;
rnorm2 = cholmod_norm_dense (R, 0, cm) ;
resid2 = rnorm2 / axbnorm ;
}
cholmod_free_dense (&R, cm) ;
/* ---------------------------------------------------------------------- */
/* print results */
/* ---------------------------------------------------------------------- */
anz = cm->anz ;
for (i = 0 ; i < CHOLMOD_MAXMETHODS ; i++)
{
fl = cm->method [i].fl ;
xlnz = cm->method [i].lnz ;
cm->method [i].fl = -1 ;
cm->method [i].lnz = -1 ;
ordering = cm->method [i].ordering ;
if (fl >= 0)
{
printf ("Ordering: ") ;
if (ordering == CHOLMOD_POSTORDERED) printf ("postordered ") ;
if (ordering == CHOLMOD_NATURAL) printf ("natural ") ;
if (ordering == CHOLMOD_GIVEN) printf ("user ") ;
if (ordering == CHOLMOD_AMD) printf ("AMD ") ;
if (ordering == CHOLMOD_METIS) printf ("METIS ") ;
if (ordering == CHOLMOD_NESDIS) printf ("NESDIS ") ;
if (xlnz > 0)
{
printf ("fl/lnz %10.1f", fl / xlnz) ;
}
if (anz > 0)
{
printf (" lnz/anz %10.1f", xlnz / anz) ;
}
printf ("\n") ;
}
}
printf ("ints in L: %15.0f, doubles in L: %15.0f\n",
(double) isize, (double) xsize) ;
printf ("factor flops %g nnz(L) %15.0f (w/no amalgamation)\n",
cm->fl, cm->lnz) ;
if (A->stype == 0)
{
printf ("nnz(A): %15.0f\n", cm->anz) ;
}
else
{
printf ("nnz(A*A'): %15.0f\n", cm->anz) ;
}
if (cm->lnz > 0)
{
printf ("flops / nnz(L): %8.1f\n", cm->fl / cm->lnz) ;
}
if (anz > 0)
{
printf ("nnz(L) / nnz(A): %8.1f\n", cm->lnz / cm->anz) ;
}
printf ("analyze cputime: %12.4f\n", ta) ;
printf ("factor cputime: %12.4f mflop: %8.1f\n", tf,
(tf == 0) ? 0 : (1e-6*cm->fl / tf)) ;
printf ("solve cputime: %12.4f mflop: %8.1f\n", ts [0],
(ts [0] == 0) ? 0 : (1e-6*4*cm->lnz / ts [0])) ;
printf ("overall cputime: %12.4f mflop: %8.1f\n",
tot, (tot == 0) ? 0 : (1e-6 * (cm->fl + 4 * cm->lnz) / tot)) ;
printf ("solve cputime: %12.4f mflop: %8.1f (%d trials)\n", ts [1],
(ts [1] == 0) ? 0 : (1e-6*4*cm->lnz / ts [1]), NTRIALS) ;
printf ("solve2 cputime: %12.4f mflop: %8.1f (%d trials)\n", ts [2],
(ts [2] == 0) ? 0 : (1e-6*4*cm->lnz / ts [2]), NTRIALS) ;
printf ("peak memory usage: %12.0f (MB)\n",
(double) (cm->memory_usage) / 1048576.) ;
printf ("residual (|Ax-b|/(|A||x|+|b|)): ") ;
for (method = 0 ; method <= 3 ; method++)
{
printf ("%8.2e ", resid [method]) ;
}
printf ("\n") ;
if (resid2 >= 0)
{
printf ("residual %8.1e (|Ax-b|/(|A||x|+|b|))"
" after iterative refinement\n", resid2) ;
}
printf ("rcond %8.1e\n\n", rcond) ;
if (L_is_super)
{
cholmod_gpu_stats (cm) ;
}
cholmod_free_factor (&L, cm) ;
cholmod_free_dense (&X, cm) ;
/* ---------------------------------------------------------------------- */
/* free matrices and finish CHOLMOD */
/* ---------------------------------------------------------------------- */
cholmod_free_sparse (&A, cm) ;
cholmod_free_dense (&B, cm) ;
cholmod_finish (cm) ;
return (0) ;
}
static int free_sparse(cholmod_sparse** A, cholmod_common* c) {
return cholmod_free_sparse(A, c);
}
void MultivariateFNormalSufficientSparse::set_Sigma(
const SparseMatrix<double>& Sigma)
{
if (Sigma.cols() != Sigma.rows()) {
IMP_THROW("need a square matrix!", ModelException);
}
//std::cout << "set_sigma" << std::endl;
if (Sigma_) cholmod_free_sparse(&Sigma_, c_);
cholmod_sparse A(Eigen::viewAsCholmod(
Sigma.selfadjointView<Eigen::Upper>()));
Sigma_=cholmod_copy_sparse(&A, c_);
//cholmod_print_sparse(Sigma_,"Sigma",c_);
IMP_LOG(TERSE, "MVNsparse: set Sigma to new matrix" << std::endl);
IMP_LOG(TERSE, "MVNsparse: computing Cholesky decomposition"
<< std::endl);
// compute Cholesky decomposition for determinant and inverse
//c_->final_asis=1; // setup LDLT calculation
//c_->supernodal = CHOLMOD_SIMPLICIAL;
// convert matrix to cholmod format
//symbolic and numeric factorization
L_ = cholmod_analyze(Sigma_, c_);
int success = cholmod_factorize(Sigma_, L_, c_);
//cholmod_print_factor(L_,"L",c_);
if (success == 0 || L_->minor < L_->n)
IMP_THROW("Sigma matrix is not positive semidefinite!",
ModelException);
// determinant and derived constants
cholmod_factor *Lcp(cholmod_copy_factor(L_, c_));
cholmod_sparse *Lsp(cholmod_factor_to_sparse(Lcp,c_));
double logDetSigma=0;
if ((Lsp->itype != CHOLMOD_INT) &&
(Lsp->xtype != CHOLMOD_REAL))
IMP_THROW("types are not int and real, update them here first",
ModelException);
int *p=(int*) Lsp->p;
double *x=(double*) Lsp->x;
for (size_t i=0; i < (size_t) M_; ++i)
logDetSigma += std::log(x[p[i]]);
cholmod_free_sparse(&Lsp,c_);
cholmod_free_factor(&Lcp,c_);
IMP_LOG(TERSE, "MVNsparse: log det(Sigma) = "
<< logDetSigma << std::endl);
IMP_LOG(TERSE, "MVNsparse: det(Sigma) = "
<< exp(logDetSigma) << std::endl);
norm_= std::pow(2*IMP::PI, -double(N_*M_)/2.0)
* exp(-double(N_)/2.0*logDetSigma);
lnorm_=double(N_*M_)/2 * log(2*IMP::PI) + double(N_)/2 * logDetSigma;
IMP_LOG(TERSE, "MVNsparse: norm = " << norm_ << " lnorm = "
<< lnorm_ << std::endl);
//inverse
IMP_LOG(TERSE, "MVNsparse: solving for inverse" << std::endl);
cholmod_sparse* id = cholmod_speye(M_,M_,CHOLMOD_REAL,c_);
if (P_) cholmod_free_sparse(&P_, c_);
P_ = cholmod_spsolve(CHOLMOD_A, L_, id, c_);
cholmod_free_sparse(&id, c_);
if (!P_) IMP_THROW("Unable to solve for inverse!", ModelException);
//WP
IMP_LOG(TERSE, "MVNsparse: solving for PW" << std::endl);
if (PW_) cholmod_free_sparse(&PW_, c_);
PW_ = cholmod_spsolve(CHOLMOD_A, L_, W_, c_);
if (!PW_) IMP_THROW("Unable to solve for PW!", ModelException);
IMP_LOG(TERSE, "MVNsparse: done" << std::endl);
}
/**
* colSums(), colMeans(), rowSums() and rowMeans() for all sparce *gCMatrix()es
* @param x a ?gCMatrix, i.e. sparse column-compressed Matrix
* @param NArm logical indicating if NA's should be remove 'na.rm' in R
* @param spRes logical = 'sparseResult' indicating if result should be sparse
* @param trans logical: TRUE <==> row[Sums/Means] <==> compute col*s( t(x) )
* @param means logical: TRUE <==> compute [row/col]Means() , not *Sums()
*/
SEXP gCMatrix_colSums(SEXP x, SEXP NArm, SEXP spRes, SEXP trans, SEXP means)
{
int mn = asLogical(means), sp = asLogical(spRes), tr = asLogical(trans);
/* cholmod_sparse: drawback of coercing lgC to double: */
CHM_SP cx = AS_CHM_SP__(x);
R_CheckStack();
if (tr) {
cholmod_sparse *cxt = cholmod_transpose(cx, (int)cx->xtype, &c);
cx = cxt;
}
/* everything else *after* the above potential transpose : */
int j, nc = cx->ncol;
int *xp = (int *)(cx -> p);
#ifdef _has_x_slot_
int na_rm = asLogical(NArm), // can have NAs only with an 'x' slot
i, dnm = 0/*Wall*/;
double *xx = (double *)(cx -> x);
#endif
// result value: sparseResult (==> "*sparseVector") or dense (atomic)vector
SEXP ans = PROTECT(sp ? NEW_OBJECT(MAKE_CLASS(SparseResult_class))
: allocVector(SXP_ans, nc));
if (sp) { // sparseResult, i.e. *sparseVector (never allocating length-nc)
int nza, i1, i2, p, *ai;
Type_ans *ax;
for (j = 0, nza = 0; j < nc; j++)
if(xp[j] < xp[j + 1])
nza++;
ai = INTEGER(ALLOC_SLOT(ans, Matrix_iSym, INTSXP, nza));
ax = STYP_ans(ALLOC_SLOT(ans, Matrix_xSym, SXP_ans, nza));
SET_SLOT(ans, Matrix_lengthSym, ScalarInteger(nc));
i2 = xp[0];
for (j = 1, p = 0; j <= nc; j++) {
/* j' =j+1, since 'i' slot will be 1-based */
i1 = i2; i2 = xp[j];
if(i1 < i2) {
Type_ans sum;
ColSUM_column(i1,i2, sum);
ai[p] = j;
ax[p++] = sum;
}
}
}
else { /* "numeric" (non sparse) result */
Type_ans *a = STYP_ans(ans);
for (j = 0; j < nc; j++) {
ColSUM_column(xp[j], xp[j + 1], a[j]);
}
}
if (tr) cholmod_free_sparse(&cx, &c);
if (!sp) {
SEXP nms = VECTOR_ELT(GET_SLOT(x, Matrix_DimNamesSym), tr ? 0 : 1);
if (!isNull(nms))
setAttrib(ans, R_NamesSymbol, duplicate(nms));
}
UNPROTECT(1);
return ans;
}
int klu_cholmod
(
/* inputs */
int n, /* A is n-by-n */
int Ap [ ], /* column pointers */
int Ai [ ], /* row indices */
/* outputs */
int Perm [ ], /* fill-reducing permutation */
/* user-defined */
klu_common *Common /* user-defined data is in Common->user_data */
)
{
double one [2] = {1,0}, zero [2] = {0,0}, lnz = 0 ;
cholmod_sparse Amatrix, *A, *AT, *S ;
cholmod_factor *L ;
cholmod_common cm ;
int *P ;
int k, symmetric ;
if (Ap == NULL || Ai == NULL || Perm == NULL || n < 0)
{
/* invalid inputs */
return (0) ;
}
/* start CHOLMOD */
cholmod_start (&cm) ;
cm.supernodal = CHOLMOD_SIMPLICIAL ;
cm.print = 0 ;
/* use KLU memory management routines for CHOLMOD */
cm.malloc_memory = Common->malloc_memory ;
cm.realloc_memory = Common->realloc_memory ;
cm.calloc_memory = Common->calloc_memory ;
cm.free_memory = Common->free_memory ;
/* construct a CHOLMOD version of the input matrix A */
A = &Amatrix ;
A->nrow = n ; /* A is n-by-n */
A->ncol = n ;
A->nzmax = Ap [n] ; /* with nzmax entries */
A->packed = TRUE ; /* there is no A->nz array */
A->stype = 0 ; /* A is unsymmetric */
A->itype = CHOLMOD_INT ;
A->xtype = CHOLMOD_PATTERN ;
A->dtype = CHOLMOD_DOUBLE ;
A->nz = NULL ;
A->p = Ap ; /* column pointers */
A->i = Ai ; /* row indices */
A->x = NULL ; /* no numerical values */
A->z = NULL ;
A->sorted = FALSE ; /* columns of A are not sorted */
/* get the user_data; default is symmetric if user_data is NULL */
symmetric = (Common->user_data == NULL) ? TRUE :
(((int *) (Common->user_data)) [0] != 0) ;
/* AT = pattern of A' */
AT = cholmod_transpose (A, 0, &cm) ;
if (symmetric)
{
/* S = the symmetric pattern of A+A' */
S = cholmod_add (A, AT, one, zero, FALSE, FALSE, &cm) ;
cholmod_free_sparse (&AT, &cm) ;
if (S != NULL)
{
S->stype = 1 ;
}
}
else
{
/* S = A'. CHOLMOD will order S*S', which is A'*A */
S = AT ;
}
/* order and analyze S or S*S' */
L = cholmod_analyze (S, &cm) ;
/* copy the permutation from L to the output */
if (L != NULL)
{
P = L->Perm ;
for (k = 0 ; k < n ; k++)
{
Perm [k] = P [k] ;
}
lnz = cm.lnz ;
}
cholmod_free_sparse (&S, &cm) ;
cholmod_free_factor (&L, &cm) ;
cholmod_finish (&cm) ;
return (lnz) ;
}
int get_blocking_internal(const int n_vertices,
const int n_edges,
const SEXP NNE_R,
const bool directed,
const int MIS_method,
const int unassinged_method,
std::list<int>& seeds,
int* const blocks) {
cholmod_common cholmod_c;
cholmod_start(&cholmod_c);
cholmod_sparse* NNE = get_cholmod_NNE(n_vertices, n_edges, NNE_R, &cholmod_c);
if (!directed) {
// NNE = NNE | t(NNE)
cholmod_sparse* NNEt = cholmod_transpose(NNE, CHOLMOD_PATTERN, &cholmod_c);
cholmod_sparse* NNE_tmp = cholmod_add(NNE, NNEt, NULL, NULL, false, false, &cholmod_c);
cholmod_free_sparse(&NNEt, &cholmod_c);
NNE->i = NULL; // Remove pointer to R object before freeing memory
cholmod_free_sparse(&NNE, &cholmod_c);
NNE = NNE_tmp;
}
switch(MIS_method) {
case LEXICAL:
findMIS_in_sp_lex(NNE, seeds);
break;
case FSTPOWORDER:
case SNDPOWORDER:
case HEURISTIC:
{
std::list<int> ordering;
get_ordering(NNE, MIS_method, directed, ordering, &cholmod_c);
if (MIS_method == HEURISTIC) {
heuristic_search(NNE, ordering, seeds);
} else {
findMIS_in_sp_order(NNE, ordering, seeds);
}
}
break;
case MAXIS:
findMaxIS_in_sp(NNE, directed, seeds, &cholmod_c);
break;
default:
error("Unknown MIS method.");
}
const int* const NNE_p = static_cast<const int*>(NNE->p);
const int* const NNE_i = static_cast<const int*>(NNE->i);
int n_unassigned = n_vertices;
int block_label = 1;
for (std::list<int>::const_iterator it = seeds.begin();
it != seeds.end(); ++it, ++block_label) {
// Set block for seed
blocks[*it] = block_label;
--n_unassigned;
// Set block for adjacent to seed
const int* const a_stop = NNE_i + NNE_p[*it + 1];
for (const int* a = NNE_i + NNE_p[*it]; a != a_stop; ++a) {
blocks[*a] = block_label;
--n_unassigned;
}
}
if (unassinged_method == ADJACENT_S) {
// Assign unassigned to the block that contains
// a neighbor in the NNE. Set to negative first as
// unassigned cannot be match to another unassigned
// that just been assigned.
// If NNE is directed, it is ordered by closeness.
// I.e. the unassigned will be assigned to the blocks
// that contain their closest neighbor.
// When NNE is undirected, the matrix multiplication
// has scrambled the ordering. Then the unassigned are
// assigned to neighbors lexically.
if (directed) {
for (int i = 0; i < n_vertices; ++i) {
if (blocks[i] == 0) {
--n_unassigned;
const int* const a_stop = NNE_i + NNE_p[i + 1];
for (const int* a = NNE_i + NNE_p[i]; a != a_stop; ++a) {
if (blocks[*a] > 0) {
blocks[i] = -blocks[*a];
break;
}
}
}
}
} else {
for (int i = 0; i < n_vertices; ++i) {
if (blocks[i] == 0) {
--n_unassigned;
int lowest_adjacent = n_vertices;
const int* const a_stop = NNE_i + NNE_p[i + 1];
for (const int* a = NNE_i + NNE_p[i]; a != a_stop; ++a) {
if (*a < lowest_adjacent && blocks[*a] > 0) {
blocks[i] = -blocks[*a];
lowest_adjacent = *a;
}
}
}
}
}
for (int i = 0; i < n_vertices; ++i) {
if (blocks[i] < 0) {
blocks[i] = -blocks[i];
}
}
}
if (directed) { // This is already done for undirected case
NNE->i = NULL; // Remove pointer to R object before freeing memory
}
cholmod_free_sparse(&NNE, &cholmod_c);
cholmod_finish(&cholmod_c);
return n_unassigned;
}
/**
* Populate ans with the pointers from x and modify its scalar
* elements accordingly. Note that later changes to the contents of
* ans will change the contents of the SEXP.
*
* In most cases this function is called through the macros
* AS_CHM_SP() or AS_CHM_SP__(). It is unusual to call it directly.
*
* @param ans a CHM_SP pointer
* @param x pointer to an object that inherits from CsparseMatrix
* @param check_Udiag boolean - should a check for (and consequent
* expansion of) a unit diagonal be performed.
* @param sort_in_place boolean - if the i and x slots are to be sorted
* should they be sorted in place? If the i and x slots are pointers
* to an input SEXP they should not be modified.
*
* @return ans containing pointers to the slots of x, *unless*
* check_Udiag and x is unitriangular.
*/
CHM_SP as_cholmod_sparse(CHM_SP ans, SEXP x,
Rboolean check_Udiag, Rboolean sort_in_place)
{
static const char *valid[] = { MATRIX_VALID_Csparse, ""};
int *dims = INTEGER(GET_SLOT(x, Matrix_DimSym)),
ctype = R_check_class_etc(x, valid);
SEXP islot = GET_SLOT(x, Matrix_iSym);
if (ctype < 0) error(_("invalid class of object to as_cholmod_sparse"));
if (!isValid_Csparse(x))
error(_("invalid object passed to as_cholmod_sparse"));
memset(ans, 0, sizeof(cholmod_sparse)); /* zero the struct */
ans->itype = CHOLMOD_INT; /* characteristics of the system */
ans->dtype = CHOLMOD_DOUBLE;
ans->packed = TRUE;
/* slots always present */
ans->i = INTEGER(islot);
ans->p = INTEGER(GET_SLOT(x, Matrix_pSym));
/* dimensions and nzmax */
ans->nrow = dims[0];
ans->ncol = dims[1];
/* Allow for over-allocation of the i and x slots. Needed for
* sparse X form in lme4. Right now it looks too difficult to
* check for the length of the x slot, because of the xpt
* utility, but the lengths of x and i should agree. */
ans->nzmax = LENGTH(islot);
/* values depending on ctype */
ans->x = xpt(ctype, x);
ans->stype = stype(ctype, x);
ans->xtype = xtype(ctype);
/* are the columns sorted (increasing row numbers) ?*/
ans->sorted = check_sorted_chm(ans);
if (!(ans->sorted)) { /* sort columns */
if(sort_in_place) {
if (!cholmod_sort(ans, &c))
error(_("in_place cholmod_sort returned an error code"));
ans->sorted = 1;
}
else {
CHM_SP tmp = cholmod_copy_sparse(ans, &c);
if (!cholmod_sort(tmp, &c))
error(_("cholmod_sort returned an error code"));
#ifdef DEBUG_Matrix
/* This "triggers" exactly for return values of dtCMatrix_sparse_solve():*/
/* Don't want to translate this: want it report */
Rprintf("Note: as_cholmod_sparse() needed cholmod_sort()ing\n");
#endif
chm2Ralloc(ans, tmp);
cholmod_free_sparse(&tmp, &c);
}
}
if (check_Udiag && ctype % 3 == 2 // triangular
&& (*diag_P(x) == 'U')) { /* diagU2N(.) "in place" : */
double one[] = {1, 0};
CHM_SP eye = cholmod_speye(ans->nrow, ans->ncol, ans->xtype, &c);
CHM_SP tmp = cholmod_add(ans, eye, one, one, TRUE, TRUE, &c);
#ifdef DEBUG_Matrix_verbose /* happens quite often, e.g. in ../tests/indexing.R : */
Rprintf("Note: as_cholmod_sparse(<ctype=%d>) - diagU2N\n", ctype);
#endif
chm2Ralloc(ans, tmp);
cholmod_free_sparse(&tmp, &c);
cholmod_free_sparse(&eye, &c);
} /* else :
* NOTE: if(*diag_P(x) == 'U'), the diagonal is lost (!);
* ---- that may be ok, e.g. if we are just converting from/to Tsparse,
* but is *not* at all ok, e.g. when used before matrix products */
return ans;
}
int main (int argc, char **argv)
{
double resid, t, ta, tf, ts, tot, bnorm, xnorm, anorm, rnorm, fl, anz,
axbnorm, rnorm2, resid2 ;
FILE *f ;
cholmod_sparse *A ;
cholmod_dense *X, *B, *W, *R ;
double one [2], zero [2], minusone [2], beta [2], xlnz ;
cholmod_common Common, *cm ;
cholmod_factor *L ;
double *Bx, *Rx, *Xx ;
int i, n, isize, xsize, ordering, xtype, s, ss, lnz ;
/* ---------------------------------------------------------------------- */
/* get the file containing the input matrix */
/* ---------------------------------------------------------------------- */
ff = NULL ;
if (argc > 1)
{
if ((f = fopen (argv [1], "r")) == NULL)
{
my_handler (CHOLMOD_INVALID, __FILE__, __LINE__,
"unable to open file") ;
}
ff = f ;
}
else
{
f = stdin ;
}
/* ---------------------------------------------------------------------- */
/* start CHOLMOD and set parameters */
/* ---------------------------------------------------------------------- */
cm = &Common ;
cholmod_start (cm) ;
/* use default parameter settings, except for the error handler. This
* demo program terminates if an error occurs (out of memory, not positive
* definite, ...). It makes the demo program simpler (no need to check
* CHOLMOD error conditions). This non-default parameter setting has no
* effect on performance. */
cm->error_handler = my_handler ;
/* Note that CHOLMOD will do a supernodal LL' or a simplicial LDL' by
* default, automatically selecting the latter if flop/nnz(L) < 40. */
/* ---------------------------------------------------------------------- */
/* create basic scalars */
/* ---------------------------------------------------------------------- */
zero [0] = 0 ;
zero [1] = 0 ;
one [0] = 1 ;
one [1] = 0 ;
minusone [0] = -1 ;
minusone [1] = 0 ;
beta [0] = 1e-6 ;
beta [1] = 0 ;
/* ---------------------------------------------------------------------- */
/* read in a matrix */
/* ---------------------------------------------------------------------- */
printf ("\n---------------------------------- cholmod_demo:\n") ;
A = cholmod_read_sparse (f, cm) ;
if (ff != NULL) fclose (ff) ;
anorm = cholmod_norm_sparse (A, 0, cm) ;
xtype = A->xtype ;
printf ("norm (A,inf) = %g\n", anorm) ;
printf ("norm (A,1) = %g\n", cholmod_norm_sparse (A, 1, cm)) ;
cholmod_print_sparse (A, "A", cm) ;
if (A->nrow > A->ncol)
{
/* Transpose A so that A'A+beta*I will be factorized instead */
cholmod_sparse *C = cholmod_transpose (A, 2, cm) ;
cholmod_free_sparse (&A, cm) ;
A = C ;
printf ("transposing input matrix\n") ;
}
/* ---------------------------------------------------------------------- */
/* create an arbitrary right-hand-side */
/* ---------------------------------------------------------------------- */
n = A->nrow ;
B = cholmod_zeros (n, 1, xtype, cm) ;
Bx = B->x ;
#if GHS
{
/* b = A*ones(n,1), used by Gould, Hu, and Scott in their experiments */
cholmod_dense *X0 ;
X0 = cholmod_ones (A->ncol, 1, xtype, cm) ;
cholmod_sdmult (A, 0, one, zero, X0, B, cm) ;
cholmod_free_dense (&X0, cm) ;
}
#else
if (xtype == CHOLMOD_REAL)
{
/* real case */
for (i = 0 ; i < n ; i++)
{
double x = n ;
Bx [i] = 1 + i / x ;
}
}
else
{
/* complex case */
for (i = 0 ; i < n ; i++)
{
double x = n ;
Bx [2*i ] = 1 + i / x ; /* real part of B(i) */
Bx [2*i+1] = (x/2 - i) / (3*x) ; /* imag part of B(i) */
}
}
#endif
cholmod_print_dense (B, "B", cm) ;
bnorm = cholmod_norm_dense (B, 0, cm) ; /* max norm */
printf ("bnorm %g\n", bnorm) ;
/* ---------------------------------------------------------------------- */
/* analyze, factorize, and solve */
/* ---------------------------------------------------------------------- */
t = CPUTIME ;
L = cholmod_analyze (A, cm) ;
ta = CPUTIME - t ;
ta = MAX (ta, 0) ;
printf ("Analyze: flop %g lnz %g\n", cm->fl, cm->lnz) ;
if (A->stype == 0)
{
printf ("Factorizing A*A'+beta*I\n") ;
t = CPUTIME ;
cholmod_factorize_p (A, beta, NULL, 0, L, cm) ;
tf = CPUTIME - t ;
tf = MAX (tf, 0) ;
}
else
{
printf ("Factorizing A\n") ;
t = CPUTIME ;
cholmod_factorize (A, L, cm) ;
tf = CPUTIME - t ;
tf = MAX (tf, 0) ;
}
t = CPUTIME ;
X = cholmod_solve (CHOLMOD_A, L, B, cm) ;
ts = CPUTIME - t ;
ts = MAX (ts, 0) ;
tot = ta + tf + ts ;
/* ---------------------------------------------------------------------- */
/* compute the residual */
/* ---------------------------------------------------------------------- */
if (A->stype == 0)
{
/* (AA'+beta*I)x=b is the linear system that was solved */
/* W = A'*X */
W = cholmod_allocate_dense (A->ncol, 1, A->ncol, xtype, cm) ;
cholmod_sdmult (A, 2, one, zero, X, W, cm) ;
/* R = B - beta*X */
R = cholmod_zeros (n, 1, xtype, cm) ;
Rx = R->x ;
Xx = X->x ;
if (xtype == CHOLMOD_REAL)
{
for (i = 0 ; i < n ; i++)
{
Rx [i] = Bx [i] - beta [0] * Xx [i] ;
}
}
else
{
/* complex case */
for (i = 0 ; i < n ; i++)
{
Rx [2*i ] = Bx [2*i ] - beta [0] * Xx [2*i ] ;
Rx [2*i+1] = Bx [2*i+1] - beta [0] * Xx [2*i+1] ;
}
}
/* R = A*W - R */
cholmod_sdmult (A, 0, one, minusone, W, R, cm) ;
cholmod_free_dense (&W, cm) ;
}
else
{
/* Ax=b was factorized and solved, R = B-A*X */
R = cholmod_copy_dense (B, cm) ;
cholmod_sdmult (A, 0, minusone, one, X, R, cm) ;
}
rnorm = cholmod_norm_dense (R, 0, cm) ; /* max abs. entry */
xnorm = cholmod_norm_dense (X, 0, cm) ; /* max abs. entry */
axbnorm = (anorm * xnorm + bnorm + ((n == 0) ? 1 : 0)) ;
resid = rnorm / axbnorm ;
/* ---------------------------------------------------------------------- */
/* iterative refinement (real symmetric case only) */
/* ---------------------------------------------------------------------- */
resid2 = -1 ;
if (A->stype != 0 && A->xtype == CHOLMOD_REAL)
{
cholmod_dense *R2 ;
/* R2 = A\(B-A*X) */
R2 = cholmod_solve (CHOLMOD_A, L, R, cm) ;
/* compute X = X + A\(B-A*X) */
Xx = X->x ;
Rx = R2->x ;
for (i = 0 ; i < n ; i++)
{
Xx [i] = Xx [i] + Rx [i] ;
}
cholmod_free_dense (&R2, cm) ;
cholmod_free_dense (&R, cm) ;
/* compute the new residual, R = B-A*X */
R = cholmod_copy_dense (B, cm) ;
cholmod_sdmult (A, 0, minusone, one, X, R, cm) ;
rnorm2 = cholmod_norm_dense (R, 0, cm) ;
resid2 = rnorm2 / axbnorm ;
}
cholmod_free_dense (&R, cm) ;
/* ---------------------------------------------------------------------- */
/* print results */
/* ---------------------------------------------------------------------- */
cholmod_print_factor (L, "L", cm) ;
/* determine the # of integers's and reals's in L. See cholmod_free */
if (L->is_super)
{
s = L->nsuper + 1 ;
xsize = L->xsize ;
ss = L->ssize ;
isize =
n /* L->Perm */
+ n /* L->ColCount, nz in each column of 'pure' L */
+ s /* L->pi, column pointers for L->s */
+ s /* L->px, column pointers for L->x */
+ s /* L->super, starting column index of each supernode */
+ ss ; /* L->s, the pattern of the supernodes */
}
else
{
/* this space can increase if you change parameters to their non-
* default values (cm->final_pack, for example). */
lnz = L->nzmax ;
xsize = lnz ;
isize =
n /* L->Perm */
+ n /* L->ColCount, nz in each column of 'pure' L */
+ n+1 /* L->p, column pointers */
+ lnz /* L->i, integer row indices */
+ n /* L->nz, nz in each column of L */
+ n+2 /* L->next, link list */
+ n+2 ; /* L->prev, link list */
}
anz = cm->anz ;
for (i = 0 ; i < CHOLMOD_MAXMETHODS ; i++)
{
fl = cm->method [i].fl ;
xlnz = cm->method [i].lnz ;
cm->method [i].fl = -1 ;
cm->method [i].lnz = -1 ;
ordering = cm->method [i].ordering ;
if (fl >= 0)
{
printf ("Ordering: ") ;
if (ordering == CHOLMOD_POSTORDERED) printf ("postordered ") ;
if (ordering == CHOLMOD_NATURAL) printf ("natural ") ;
if (ordering == CHOLMOD_GIVEN) printf ("user ") ;
if (ordering == CHOLMOD_AMD) printf ("AMD ") ;
if (ordering == CHOLMOD_METIS) printf ("METIS ") ;
if (ordering == CHOLMOD_NESDIS) printf ("NESDIS ") ;
if (xlnz > 0)
{
printf ("fl/lnz %10.1f", fl / xlnz) ;
}
if (anz > 0)
{
printf (" lnz/anz %10.1f", xlnz / anz) ;
}
printf ("\n") ;
}
}
printf ("ints in L: %d, doubles in L: %d\n", isize, xsize) ;
printf ("factor flops %g nnz(L) %15.0f (w/no amalgamation)\n",
cm->fl, cm->lnz) ;
if (A->stype == 0)
{
printf ("nnz(A): %15.0f\n", cm->anz) ;
}
else
{
printf ("nnz(A*A'): %15.0f\n", cm->anz) ;
}
if (cm->lnz > 0)
{
printf ("flops / nnz(L): %8.1f\n", cm->fl / cm->lnz) ;
}
if (anz > 0)
{
printf ("nnz(L) / nnz(A): %8.1f\n", cm->lnz / cm->anz) ;
}
printf ("analyze cputime: %12.4f\n", ta) ;
printf ("factor cputime: %12.4f mflop: %8.1f\n", tf,
(tf == 0) ? 0 : (1e-6*cm->fl / tf)) ;
printf ("solve cputime: %12.4f mflop: %8.1f\n", ts,
(ts == 0) ? 0 : (1e-6*4*cm->lnz / ts)) ;
printf ("overall cputime: %12.4f mflop: %8.1f\n",
tot, (tot == 0) ? 0 : (1e-6 * (cm->fl + 4 * cm->lnz) / tot)) ;
printf ("peak memory usage: %12.0f (MB)\n",
(double) (cm->memory_usage) / 1048576.) ;
printf ("residual %8.1e (|Ax-b|/(|A||x|+|b|))\n", resid) ;
if (resid2 >= 0)
{
printf ("residual %8.1e (|Ax-b|/(|A||x|+|b|))"
" after iterative refinement\n", resid2) ;
}
printf ("rcond %8.1e\n\n", cholmod_rcond (L, cm)) ;
cholmod_free_factor (&L, cm) ;
cholmod_free_dense (&X, cm) ;
/* ---------------------------------------------------------------------- */
/* free matrices and finish CHOLMOD */
/* ---------------------------------------------------------------------- */
cholmod_free_sparse (&A, cm) ;
cholmod_free_dense (&B, cm) ;
cholmod_finish (cm) ;
return (0) ;
}
bool SparseOptimizerIncremental::updateInitialization(HyperGraph::VertexSet& vset, HyperGraph::EdgeSet& eset)
{
if (batchStep) {
return SparseOptimizerOnline::updateInitialization(vset, eset);
}
for (HyperGraph::VertexSet::iterator it = vset.begin(); it != vset.end(); ++it) {
OptimizableGraph::Vertex* v = static_cast<OptimizableGraph::Vertex*>(*it);
v->clearQuadraticForm(); // be sure that b is zero for this vertex
}
// get the touched vertices
_touchedVertices.clear();
for (HyperGraph::EdgeSet::iterator it = eset.begin(); it != eset.end(); ++it) {
OptimizableGraph::Edge* e = static_cast<OptimizableGraph::Edge*>(*it);
OptimizableGraph::Vertex* v1 = static_cast<OptimizableGraph::Vertex*>(e->vertices()[0]);
OptimizableGraph::Vertex* v2 = static_cast<OptimizableGraph::Vertex*>(e->vertices()[1]);
if (! v1->fixed())
_touchedVertices.insert(v1);
if (! v2->fixed())
_touchedVertices.insert(v2);
}
//cerr << PVAR(_touchedVertices.size()) << endl;
// updating the internal structures
std::vector<HyperGraph::Vertex*> newVertices;
newVertices.reserve(vset.size());
_activeVertices.reserve(_activeVertices.size() + vset.size());
_activeEdges.reserve(_activeEdges.size() + eset.size());
for (HyperGraph::EdgeSet::iterator it = eset.begin(); it != eset.end(); ++it)
_activeEdges.push_back(static_cast<OptimizableGraph::Edge*>(*it));
//cerr << "updating internal done." << endl;
// update the index mapping
size_t next = _ivMap.size();
for (HyperGraph::VertexSet::iterator it = vset.begin(); it != vset.end(); ++it) {
OptimizableGraph::Vertex* v=static_cast<OptimizableGraph::Vertex*>(*it);
if (! v->fixed()){
if (! v->marginalized()){
v->setHessianIndex(next);
_ivMap.push_back(v);
newVertices.push_back(v);
_activeVertices.push_back(v);
next++;
}
else // not supported right now
abort();
}
else {
v->setHessianIndex(-1);
}
}
//cerr << "updating index mapping done." << endl;
// backup the tempindex and prepare sorting structure
VertexBackup backupIdx[_touchedVertices.size()];
memset(backupIdx, 0, sizeof(VertexBackup) * _touchedVertices.size());
int idx = 0;
for (HyperGraph::VertexSet::iterator it = _touchedVertices.begin(); it != _touchedVertices.end(); ++it) {
OptimizableGraph::Vertex* v = static_cast<OptimizableGraph::Vertex*>(*it);
backupIdx[idx].hessianIndex = v->hessianIndex();
backupIdx[idx].vertex = v;
backupIdx[idx].hessianData = v->hessianData();
++idx;
}
sort(backupIdx, backupIdx + _touchedVertices.size()); // sort according to the hessianIndex which is the same order as used later by the optimizer
for (int i = 0; i < idx; ++i) {
backupIdx[i].vertex->setHessianIndex(i);
}
//cerr << "backup tempindex done." << endl;
// building the structure of the update
_updateMat.clear(true); // get rid of the old matrix structure
_updateMat.rowBlockIndices().clear();
_updateMat.colBlockIndices().clear();
_updateMat.blockCols().clear();
// placing the current stuff in _updateMat
MatrixXd* lastBlock = 0;
int sizePoses = 0;
for (int i = 0; i < idx; ++i) {
OptimizableGraph::Vertex* v = backupIdx[i].vertex;
int dim = v->dimension();
sizePoses+=dim;
_updateMat.rowBlockIndices().push_back(sizePoses);
_updateMat.colBlockIndices().push_back(sizePoses);
_updateMat.blockCols().push_back(SparseBlockMatrix<MatrixXd>::IntBlockMap());
int ind = v->hessianIndex();
//cerr << PVAR(ind) << endl;
if (ind >= 0) {
MatrixXd* m = _updateMat.block(ind, ind, true);
v->mapHessianMemory(m->data());
lastBlock = m;
}
}
lastBlock->diagonal().array() += 1e-6; // HACK to get Eigen value > 0
for (HyperGraph::EdgeSet::const_iterator it = eset.begin(); it != eset.end(); ++it) {
OptimizableGraph::Edge* e = static_cast<OptimizableGraph::Edge*>(*it);
OptimizableGraph::Vertex* v1 = (OptimizableGraph::Vertex*) e->vertices()[0];
OptimizableGraph::Vertex* v2 = (OptimizableGraph::Vertex*) e->vertices()[1];
int ind1 = v1->hessianIndex();
if (ind1 == -1)
continue;
int ind2 = v2->hessianIndex();
if (ind2 == -1)
continue;
bool transposedBlock = ind1 > ind2;
if (transposedBlock) // make sure, we allocate the upper triangular block
swap(ind1, ind2);
MatrixXd* m = _updateMat.block(ind1, ind2, true);
e->mapHessianMemory(m->data(), 0, 1, transposedBlock);
}
// build the system into _updateMat
for (HyperGraph::EdgeSet::iterator it = eset.begin(); it != eset.end(); ++it) {
OptimizableGraph::Edge * e = static_cast<OptimizableGraph::Edge*>(*it);
e->computeError();
}
for (HyperGraph::EdgeSet::iterator it = eset.begin(); it != eset.end(); ++it) {
OptimizableGraph::Edge* e = static_cast<OptimizableGraph::Edge*>(*it);
e->linearizeOplus();
}
for (HyperGraph::EdgeSet::iterator it = eset.begin(); it != eset.end(); ++it) {
OptimizableGraph::Edge* e = static_cast<OptimizableGraph::Edge*>(*it);
e->constructQuadraticForm();
}
// restore the original data for the vertex
for (int i = 0; i < idx; ++i) {
backupIdx[i].vertex->setHessianIndex(backupIdx[i].hessianIndex);
if (backupIdx[i].hessianData)
backupIdx[i].vertex->mapHessianMemory(backupIdx[i].hessianData);
}
// update the structure of the real block matrix
bool solverStatus = _algorithm->updateStructure(newVertices, eset);
bool updateStatus = computeCholeskyUpdate();
if (! updateStatus) {
cerr << "Error while computing update" << endl;
}
cholmod_sparse* updateAsSparseFactor = cholmod_factor_to_sparse(_cholmodFactor, &_cholmodCommon);
// convert CCS update by permuting back to the permutation of L
if (updateAsSparseFactor->nzmax > _permutedUpdate->nzmax) {
//cerr << "realloc _permutedUpdate" << endl;
cholmod_reallocate_triplet(updateAsSparseFactor->nzmax, _permutedUpdate, &_cholmodCommon);
}
_permutedUpdate->nnz = 0;
_permutedUpdate->nrow = _permutedUpdate->ncol = _L->n;
{
int* Ap = (int*)updateAsSparseFactor->p;
int* Ai = (int*)updateAsSparseFactor->i;
double* Ax = (double*)updateAsSparseFactor->x;
int* Bj = (int*)_permutedUpdate->j;
int* Bi = (int*)_permutedUpdate->i;
double* Bx = (double*)_permutedUpdate->x;
for (size_t c = 0; c < updateAsSparseFactor->ncol; ++c) {
const int& rbeg = Ap[c];
const int& rend = Ap[c+1];
int cc = c / slamDimension;
int coff = c % slamDimension;
const int& cbase = backupIdx[cc].vertex->colInHessian();
const int& ccol = _perm(cbase + coff);
for (int j = rbeg; j < rend; j++) {
const int& r = Ai[j];
const double& val = Ax[j];
int rr = r / slamDimension;
int roff = r % slamDimension;
const int& rbase = backupIdx[rr].vertex->colInHessian();
int row = _perm(rbase + roff);
int col = ccol;
if (col > row) // lower triangular entry
swap(col, row);
Bi[_permutedUpdate->nnz] = row;
Bj[_permutedUpdate->nnz] = col;
Bx[_permutedUpdate->nnz] = val;
++_permutedUpdate->nnz;
}
}
}
cholmod_free_sparse(&updateAsSparseFactor, &_cholmodCommon);
#if 0
cholmod_sparse* updatePermuted = cholmod_triplet_to_sparse(_permutedUpdate, _permutedUpdate->nnz, &_cholmodCommon);
//writeCCSMatrix("update-permuted.txt", updatePermuted->nrow, updatePermuted->ncol, (int*)updatePermuted->p, (int*)updatePermuted->i, (double*)updatePermuted->x, false);
_solverInterface->choleskyUpdate(updatePermuted);
cholmod_free_sparse(&updatePermuted, &_cholmodCommon);
#else
convertTripletUpdateToSparse();
_solverInterface->choleskyUpdate(_permutedUpdateAsSparse);
#endif
return solverStatus;
}
void tele2d::computeVectorField(){
unsigned time1, time2, time3 ;
time1 = clock() ;
std::vector<std::vector<double2>> allcurves = curves ;
vector_field.clear() ;
vector_field.resize(resolution*resolution) ;
// delete too short curves
for( int i=0; i<allcurves.size(); ++i ){
if( allcurves[i].size() < 5 )
allcurves.erase( allcurves.begin() + i ) ;
}
if( allcurves.size() == 0 ){
std::cout<<"no valid curves!" ;
exit(1) ;
}
// mark constrained vertices
constrained_vertices_mark.clear() ;
for( int i=0; i<resolution; ++i ) {
std::vector<int> a ;
for( int j=0;j<resolution; ++j )
a.push_back(0) ;
constrained_vertices_mark.push_back(a) ;
}
for( int i=0; i<allcurves.size(); ++ i){
for( int j =0; j<allcurves[i].size(); ++ j){
// get x index of closest vertices
float x = allcurves[i][j].x * resolution - 0.5 ;
int ix ;
if( x-floor(x) < 0.5 ) ix = floor(x) ;
else ix = ceil( x ) ;
// get y index of closest vertices
float y = allcurves[i][j].y * resolution - 0.5 ;
int iy ;
if( y-floor(y) < 0.5 ) iy = floor(y) ;
else iy = ceil( y ) ;
if( ix < 0 ) ix = 0;
if( ix > resolution-1) ix = resolution -1;
if( iy < 0 ) iy = 0;
if( iy > resolution-1) iy = resolution -1;
constrained_vertices_mark[ix][iy] = 1 ;
}
}
// compute b
std::vector<double2> b ;
b.resize(resolution*resolution) ;
for( int i=0; i<resolution; ++i ){
for( int j=0; j<resolution; ++j){
if(constrained_vertices_mark[i][j] == 0 ){
b[i+j*resolution].x = 0;
b[i+j*resolution].y = 0;
continue ;
}
// otherwise, the vertex indexed by (i,j) is constrained
double vx = ((double)i+0.5)/(double)resolution ;
double vy = ((double)j+0.5)/(double)resolution ;
// search for the closest points
int curveid_record = 0;
int pointid_record = 0;
double mindis = 1000.0f ;
for( int curveid=0; curveid<allcurves.size(); ++curveid ){
for( int pointid=0; pointid<allcurves[curveid].size(); ++pointid ){
double quadratic_dis = ( allcurves[curveid][pointid].x - vx )*( allcurves[curveid][pointid].x - vx ) + ( allcurves[curveid][pointid].y - vy )*( allcurves[curveid][pointid].y - vy ) ;
if( quadratic_dis < mindis ){
mindis = quadratic_dis ;
curveid_record = curveid ;
pointid_record = pointid ;
}
}
}
// compute the vector of the vertex indexed by (i,j)
int pid1 = pointid_record-1 > 0 ? pointid_record-1 : 0 ;
int pid2 = pointid_record+1 < allcurves[curveid_record].size()-1 ? pointid_record+1 : allcurves[curveid_record].size()-1;
double2 vector_of_vertex ;
vector_of_vertex.x = allcurves[curveid_record][pid2].x - allcurves[curveid_record][pid1].x ;
vector_of_vertex.y = allcurves[curveid_record][pid2].y - allcurves[curveid_record][pid1].y ;
double norm = sqrt( vector_of_vertex.x * vector_of_vertex.x + vector_of_vertex.y * vector_of_vertex.y) ;
vector_of_vertex.x /= norm ;
vector_of_vertex.y /= norm ;
assert( norm > 0 && norm < 1) ;
//std::cout<<"norm "<<norm<<std::endl;
b[i+j*resolution ] = vector_of_vertex ;
}
}
// compute Pb
std::vector<double2> Pb = b ;
for( int i=0; i<Pb.size(); ++i ){
Pb[i].x *= 1.0e8 ;
Pb[i].y *= 1.0e8 ;
}
// compute L+P
int vnum = resolution*resolution ;
sparse_matrix L_add_P(vnum) ; // create a sparse matrix of vnum rows
// L_add_P <- D - W
for( int id_x =0; id_x<resolution; ++id_x ){
for( int id_y =0; id_y<resolution; ++id_y ){
int vid = id_x + id_y * resolution ;
if( id_x != 0 && id_x != resolution-1 && id_y != 0 && id_y != resolution-1 ){ // inner area
//L_add_P[ vid + vid*vnum] += 6.8284 ;
L_add_P.pluse(vid,vid,6.8284 ) ;
int neibour_id_1 = id_x + id_y * resolution - 1 ;
int neibour_id_2 = id_x + id_y * resolution + 1 ;
int neibour_id_3 = id_x + (id_y-1) * resolution ;
int neibour_id_4 = id_x + (id_y+1) * resolution ;
int neibour_id_5 = id_x + (id_y+1) * resolution - 1 ;
int neibour_id_6 = id_x + (id_y+1) * resolution + 1 ;
int neibour_id_7 = id_x + (id_y-1) * resolution - 1 ;
int neibour_id_8 = id_x + (id_y-1) * resolution + 1 ;
//L_add_P[neibour_id_1+vid*vnum] -= 1 ;
L_add_P.pluse(vid,neibour_id_1, -1 ) ;
//L_add_P[neibour_id_2+vid*vnum] -= 1 ;
L_add_P.pluse(vid,neibour_id_2, -1 ) ;
//L_add_P[neibour_id_3+vid*vnum] -= 1 ;
L_add_P.pluse(vid,neibour_id_3, -1 ) ;
//L_add_P[neibour_id_4+vid*vnum] -= 1 ;
L_add_P.pluse(vid,neibour_id_4, -1 ) ;
//L_add_P[neibour_id_5+vid*vnum] -= 0.7071;
L_add_P.pluse(vid,neibour_id_5, -0.7071 ) ;
//L_add_P[neibour_id_6+vid*vnum] -= 0.7071;
L_add_P.pluse(vid,neibour_id_6, -0.7071 ) ;
//L_add_P[neibour_id_7+vid*vnum] -= 0.7071;
L_add_P.pluse(vid,neibour_id_7, -0.7071 ) ;
//L_add_P[neibour_id_8+vid*vnum] -= 0.7071;
L_add_P.pluse(vid,neibour_id_8, -0.7071 ) ;
}
else if((id_x == 0 || id_x==resolution-1) && (id_y == 0 || id_y==resolution-1) ){ // coners
//L_add_P[ vid + vid*vnum] += 2.7071 ;
L_add_P.pluse(vid,vid, 2.7071 ) ;
int neibour_id_1 = ( id_x == 0 ? ( id_x+id_y * resolution+1) : ( id_x+id_y * resolution - 1) );
int neibour_id_2 = ( id_y == 0 ? ( id_x+ (id_y+1) * resolution) : ( id_x+ (id_y-1) * resolution )) ;
int neibour_id_3 = ( id_x == 0 ? 1 : (resolution-2) ) + ( id_y == 0 ? 1 : (resolution - 2)) * resolution ;
//L_add_P[neibour_id_1+vid*vnum] -= 1 ;
L_add_P.pluse(vid,neibour_id_1, -1 ) ;
//L_add_P[neibour_id_2+vid*vnum] -= 1 ;
L_add_P.pluse(vid,neibour_id_2, -1 ) ;
//L_add_P[neibour_id_3+vid*vnum] -= 0.7071;
L_add_P.pluse(vid,neibour_id_3, -0.7071 ) ;
}
else { // boundaries
//L_add_P[ vid + vid*vnum] += 4.4142 ;
L_add_P.pluse(vid,vid, 4.4142 ) ;
int neibour_id_1, neibour_id_2, neibour_id_3, neibour_id_4, neibour_id_5 ;
if( id_x == 0){
neibour_id_1 = id_x + id_y * resolution + 1 ;
neibour_id_2 = id_x + (id_y+1) * resolution ;
neibour_id_3 = id_x + (id_y-1) * resolution ;
neibour_id_4 = id_x + (id_y+1) * resolution + 1;
neibour_id_5 = id_x + (id_y-1) * resolution + 1 ;
}
else if( id_x == resolution-1 ){
neibour_id_1 = id_x + id_y * resolution - 1 ;
neibour_id_2 = id_x + (id_y+1) * resolution ;
neibour_id_3 = id_x + (id_y-1) * resolution ;
neibour_id_4 = id_x + (id_y+1) * resolution - 1;
neibour_id_5 = id_x + (id_y-1) * resolution - 1 ;
}
else if( id_y == resolution-1 ){
neibour_id_1 = id_x + id_y * resolution + 1 ;
neibour_id_2 = id_x + id_y * resolution - 1 ;
neibour_id_3 = id_x + (id_y-1) * resolution ;
neibour_id_4 = id_x + (id_y-1) * resolution + 1;
neibour_id_5 = id_x + (id_y-1) * resolution - 1 ;
}
else {
neibour_id_1 = id_x + id_y * resolution + 1 ;
neibour_id_2 = id_x + id_y * resolution - 1 ;
neibour_id_3 = id_x + (id_y+1) * resolution ;
neibour_id_4 = id_x + (id_y+1) * resolution + 1;
neibour_id_5 = id_x + (id_y+1) * resolution - 1 ;
}
//L_add_P[neibour_id_1+vid*vnum] -= 1 ;
L_add_P.pluse(vid,neibour_id_1, -1 ) ;
//L_add_P[neibour_id_2+vid*vnum] -= 1 ;
L_add_P.pluse(vid,neibour_id_2, -1 ) ;
//L_add_P[neibour_id_3+vid*vnum] -= 1 ;
L_add_P.pluse(vid,neibour_id_3, -1 ) ;
//L_add_P[neibour_id_4+vid*vnum] -= 0.7071;
L_add_P.pluse(vid,neibour_id_4, -0.7071) ;
//L_add_P[neibour_id_5+vid*vnum] -= 0.7071;
L_add_P.pluse(vid,neibour_id_5, -0.7071 ) ;
}
}
}
// L_add_P <- D - W + P
for( int i=0; i<resolution; ++i ){
for( int j=0; j<resolution; ++j){
if(constrained_vertices_mark[i][j] == 1 ){
int vid = i + j*resolution ;
//L_add_P[vid+vid*vnum]+=1e8 ;
L_add_P.pluse(vid,vid, 1.0e8 ) ;
}
}
}
// solve the linear system with cholmod
cholmod_sparse *A ;
cholmod_dense *x, *y, *b1 ;
double one [2] = {1,0}, m1 [2] = {-1,0} ; /* basic scalars */
cholmod_factor *L ;
cholmod_common c ;
cholmod_start (&c) ;; /* start CHOLMOD */
//A = cholmod_read_sparse (pFile, &c) ; /* read in a matrix */
CMatrix *SM = new CMatrix( vnum, true, &c) ;
//for( int i=0; i<vnum; ++i ){
// for( int j=0; j<vnum; ++j ){
// if( L_add_P.getValue(j,i)!=0 )
// SM->set_coef(i, j,L_add_P.getValue(j,i) ) ;
// }
//}
for( int i=0; i<L_add_P.data.size(); ++i){
for( int j=0; j<L_add_P.data[i].size(); ++j)
SM->set_coef(L_add_P.data[i][j].row, i, L_add_P.data[i][j].val ) ;
}
A =(cholmod_sparse *) SM->get_cholmod_sparse();
time2 = clock() ;
//cholmod_print_sparse (A, "A", &c) ; /* print the matrix */
if (A == NULL || A->stype == 0) /* A must be symmetric */
{
cholmod_free_sparse (&A, &c) ;
cholmod_finish (&c) ;
std::cout << "fail to load the matrix or it's not symmeric!"<<std::endl;
exit(1) ;
}
b1 = cholmod_zeros(vnum, 1, CHOLMOD_REAL, &c);
// --------------------- x demension -----------------------
for( int i =0 ;i<Pb.size(); ++i ){
((double*)(b1->x))[i] = Pb[i].x ;
}
L = cholmod_analyze (A, &c) ; /* analyze */
cholmod_factorize (A, L, &c) ; /* factorize */
x = cholmod_solve (CHOLMOD_A, L, b1, &c) ; /* solve Ax=b */
// write x-values
for( int i=0; i<vector_field.size(); ++i)
vector_field[i].x = ((double*)(x->x))[i] ;
// --------------------- y demension -----------------------
for( int i =0 ;i<Pb.size(); ++i ){
((double*)(b1->x))[i] = Pb[i].y ;
}
y = cholmod_solve (CHOLMOD_A, L, b1, &c) ; /* solve Ay=b */
// write y-values
for( int i=0; i<vector_field.size(); ++i)
vector_field[i].y = ((double*)(y->x))[i] ;
cholmod_free_factor (&L, &c) ;
cholmod_free_dense (&x, &c) ;
cholmod_free_dense (&y, &c) ;
cholmod_free_dense (&b1, &c) ;
//delete L_add_P ;
delete SM ;
cholmod_finish (&c) ; /* finish CHOLMOD */
double normx2, normy2 ;
normx2 = normy2 = 0.0 ;
for( int i=0; i<vnum; ++i ){
normx2 += vector_field[i].x * vector_field[i].x ;
normy2 += vector_field[i].y * vector_field[i].y ;
}
//std::cout<<"|x| = "<<sqrt(normx2) <<"\n|y| = "<<sqrt(normy2) <<std::endl;
// normalize vector field
for( int i=0; i<vector_field.size(); ++i){
double norm = sqrt( vector_field[i].x * vector_field[i].x + vector_field[i].y * vector_field[i].y) ;
vector_field[i].x /= norm ;
vector_field[i].y /= norm ;
}
time3 = clock() ;
//std::cout<<"time consumed by computing A and b: " << (double)(time2-time1)/CLOCKS_PER_SEC <<" s" <<std::endl ;
//std::cout<<"time consumed by solving the system: " << (double)(time3-time2)/CLOCKS_PER_SEC <<" s" <<std::endl ;
//std::cout<<"vector field computing completed."<<std::endl; ;
int count = 0;
for( int i=0; i<L_add_P.data.size(); ++i)
count += L_add_P.data[i].size() ;
//std::cout << "nonzero number: " << count <<std::endl;
}