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