bool StiffnessSolver::SolveSystem(
SpMat &K, VX &D, VX &F, VX &R,
int DoF, VXi &q, VXi &r,
int verbose, int &info, double &rms_resid)
{
VX diag; /* diagonal vector of the L D L' decomp. */
diag.resize(DoF);
//MX K_comp = K;
int row = K.rows(), col = K.cols();
MX K_comp(row, col);
K_comp.setZero();
for (int k = 0; k < K.outerSize(); ++k)
{
for (SpMat::InnerIterator it(K, k); it; ++it)
{
int r = it.row();
int c = it.col();
double v = it.value();
K_comp(r, c) = v;
}
}
/* L D L' decomposition of K[q,q] into lower triangle of K[q,q] and diag[q] */
/* vectors F and D are unchanged */
/* not solving at this moment*/
LDLDecompPM(K_comp, DoF, diag, F, D, R, q, r, 1, 0, info);
if (info < 0)
{
fprintf(stderr, "Stiffness Matrix is not positive definite: %d negative elements\n", info);
fprintf(stderr, "found on decomp diagonal of K.\n");
fprintf(stderr, "The stucture may have mechanism and thus not stable in general\n");
fprintf(stderr, "Please Make sure that all six\n");
fprintf(stderr, "rigid body translations are restrained!\n");
return false;
}
else
{
/* LDL' back-substitution for D[q] and R[r] */
LDLDecompPM(K_comp, DoF, diag, F, D, R, q, r, 0, 1, info);
if (verbose) { fprintf(stdout, " LDL' RMS residual:"); }
rms_resid = info = 1;
do {
/* improve solution for D[q] and R[r] */
LDLImprovePM(K_comp, DoF, diag, F, D, R, q, r, rms_resid, info);
if (verbose) { fprintf(stdout, "%9.2e", rms_resid); }
} while (info);
if (verbose) fprintf(stdout, "LDL^t Solving completed\n");
}
return true;
}
Mat solveQurdOpt(SpMat L, SpMat C, Mat alpha_star){
//
cout << "solving quadratic optimization proble .............." << endl;
double lambda = 0.000001;
SpMat D(L.rows(), L.cols());
D.setIdentity();
//cout << D << endl;
alpha_star = matlab_colVector(alpha_star);
MatrixXd as_dense;
cv2eigen(alpha_star, as_dense);
SpMat b = as_dense.sparseView();
SpMat A, alpha;
A = L + C + lambda * D;
b = C * b;
//cout << b << endl;
Eigen::SimplicialLLT<SpMat> solver;
//Eigen::SimplicialLDLT<SpMat> solver;
//Eigen::SparseQR<Eigen::SparseMatrix<double>> solver;
//Eigen::BiCGSTAB<SpMat> solver;
solver.compute(A);
if (solver.info() != Eigen::Success) {
cout << "decomposition failed" << endl;
}
cout << "decomposition success" << endl;
cout << "begin to solve !" << endl;
alpha = solver.solve(b);
cout << "solve success" << endl;
Mat cvAlpha;
eigen2cv(Eigen::MatrixXd(alpha), cvAlpha);
cvAlpha = cvAlpha.reshape(0, sz.width);
cvAlpha = cvAlpha.t();
showSavePicLBDM("alpha", cvAlpha, showImgLBDM, saveImgLBDM);
cvAlpha = cvAlpha*0.5 + 0.5;
cvAlpha = max(min(cvAlpha, 1.0), 0.0);
return cvAlpha;
}
Mat solveQurdOpt(SpMat L, SpMat C, VectorXd alpha_star){
//
omp_set_num_threads(1);
Eigen::setNbThreads(1);
cout << "solving quadratic optimization proble .............." << endl;
double lambda = 0.000001;
SpMat D(L.rows(), L.cols());
D.setIdentity();
SpMat b = alpha_star.sparseView();
SpMat A, alpha;
A = L + C + lambda * D;
b = C * b;
Eigen::SimplicialLDLT<SpMat> solver;
cout << "begin to factorize A" << endl;
solver.compute(A);
if(solver.info() != Eigen::Success)
cout << "decomposition failed" << endl;
else
cout << "decomposition success" << endl;
cout << "begin to solve !" << endl;
alpha = solver.solve(b);
cout << "solve success" << endl;
Mat alpha_mat;
eigen2cv(MatrixXd(alpha), alpha_mat);
alpha_mat = alpha_mat.t();
alpha_mat = alpha_mat.reshape(0, imgSize.width).t();
alpha_mat = alpha_mat * 0.5 + 0.5;
alpha_mat = max(min(alpha_mat, 1), 0);
//showSavePicLBDM("alpha", alpha_mat, showImgLBDM, saveImgLBDM);
return alpha_mat;
}
inline void computeStrongNeighbors(const double delta, const std::vector<double>& max_aff, const SpMat& C, std::set<size_t>& strongNeighbors) {
strongNeighbors.clear();
for (size_t u = 0; u < C.rows(); u++) {
for (SpMat::ConstIterator v = C.begin(u); v != C.end(u); v++) {
if (v->index() <= u) //guardo solo il triangolo superiore, senza diagonale
continue;
//v->value() è c_uv
//max_aff[u] è max{s!=u} c_us
//max_aff[v->index()] è max{s!=v} c_sv
//Quindi se c_uv >= delta*max{ max{s!=u} c_us, max{s!=v} c_sv}
if (v->value() >= delta * max(max_aff[u], max_aff[v->index()])) {
// u e v sono strong neighbors, li metto nel set
strongNeighbors.insert(u);
strongNeighbors.insert(v->index());
}
}
}
}
/* c è la matrice di adiacenza. Al posto dei suoi nonzero metto le
* affinity dei nodi, calcolate usando i TVs*/
inline void affinityMatrix(SpMat& c, std::vector<double>& max_aff, const DMat & tv) {
double cmax_i = -inf;
for (size_t i = 0; i < c.rows(); ++i) {
for (SpMat::Iterator j = c.begin(i); j != c.end(i); ++j) {
//Sfrutto la simmetria di C
if (j->index() < i)
continue;
else {
// (X_u,X_v) == (X_v,X_u) Quindi sfrutto la simmetria
j->value() = affinity_l2(tv, i, j->index());
c(j->index(), i) = j->value();
if (cmax_i < j->value()) cmax_i = j->value();
}
}
max_aff[i] = cmax_i;
}
}
bool LAMGLSSolver::isSparseOK(SpMat& m) {
/* Sparsa è OK finchè non ha il 65%+ di nonZero */
return (((double) m.nonZeros() / (double) (m.rows() * m.columns())) <= 0.65);
}