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; }
SEXP CHMfactor_to_sparse(SEXP x) { CHM_FR L = AS_CHM_FR(x), Lcp; CHM_SP Lm; R_CheckStack(); /* cholmod_factor_to_sparse changes its first argument. Make a copy */ Lcp = cholmod_copy_factor(L, &c); if (!(Lcp->is_ll)) if (!cholmod_change_factor(Lcp->xtype, 1, 0, 1, 1, Lcp, &c)) error(_("cholmod_change_factor failed with status %d"), c.status); Lm = cholmod_factor_to_sparse(Lcp, &c); cholmod_free_factor(&Lcp, &c); return chm_sparse_to_SEXP(Lm, 1/*do_free*/, -1/*uploT*/, 0/*Rkind*/, "N"/*non_unit*/, R_NilValue/*dimNames*/); }
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 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); }