void ADMMCut::CalculateX()
{
// Construct Hessian Matrix for D-Qp problem
SpMat Q;
CalculateQ(D_, Q);
SpMat H2 = Q.transpose() * Q;
SpMat H = 2 * H1_ + penalty_ * H2;
// Construct Linear coefficient for x-Qp problem
a_ = Q.transpose() * lambda_;
// Inequality constraints A*x <= b
SpMat A(6 * Fd_, 6 * Fd_);
A.setZero();
VX b(6 * Fd_);
b.setZero();
// Variable constraints x >= lb, x <= ub
VX lb(Nd_), ub(Nd_);
lb.setZero();
ub.setOnes();
qp_->solve(H, a_, A, b, W_, d_, lb, ub, x_, NULL, NULL, debug_);
}
SpMat make_C(SpMat chol_K_inv,VectorXd h2s, SpMat ZtZ){
SpMat Ki = chol_K_inv.transpose() * chol_K_inv;
SpMat C = ZtZ;
C /= (1.0 - h2s.sum());
C += Ki;
return C;
}
void ADMMCut::CalculateD()
{
// Construct Hessian Matrix for D-Qp problem
// Here, K is continuous-x weighted
ptr_stiff_->CreateGlobalK(x_);
SpMat K = *(ptr_stiff_->WeightedK());
SpMat Q = penalty_ * K.transpose() * K;
// Construct Linear coefficient for D-Qp problem
ptr_stiff_->CreateF(x_);
VX F = *(ptr_stiff_->WeightedF());
VX a = K.transpose() * lambda_ - penalty_ * K.transpose() * F;
/* 10 degree rotation tolerance, from degree to radians */
qp_->solve(Q, a, D_, Dt_tol_, Dr_tol_, debug_);
}
VectorXd sample_MME_single_diagR(
VectorXd Y,
SpMat W,
SpMat chol_C,
double pe,
SpMat chol_K_inv,
double tot_Eta_prec,
VectorXd randn_theta,
VectorXd randn_e
){
VectorXd theta_star = chol_K_inv.triangularView<Upper>().solve(randn_theta);
VectorXd e_star = randn_e / sqrt(pe);
MatrixXd W_theta_star = W * theta_star;
VectorXd Y_resid = Y - W_theta_star - e_star;
VectorXd WtRiy = W.transpose() * (Y_resid * pe);
VectorXd theta_tilda = chol_C.transpose().triangularView<Upper>().solve(chol_C.triangularView<Lower>().solve(WtRiy));
VectorXd theta = theta_tilda / tot_Eta_prec + theta_star;
return theta;
}
MatrixXd sample_MME_multiple_diagR(
MatrixXd Y,
SpMat W,
SpMat chol_C,
VectorXd pe,
MSpMat chol_K_inv,
VectorXd tot_Eta_prec,
MatrixXd randn_theta,
MatrixXd randn_e
){
MatrixXd theta_star = chol_K_inv.triangularView<Upper>().solve(randn_theta);
MatrixXd e_star = randn_e * pe.cwiseSqrt().cwiseInverse().asDiagonal();
MatrixXd W_theta_star = W * theta_star;
MatrixXd Y_resid = Y - W_theta_star - e_star;
MatrixXd WtRiy = W.transpose() * (Y_resid * pe.asDiagonal());
MatrixXd theta_tilda = chol_C.transpose().triangularView<Upper>().solve(chol_C.triangularView<Lower>().solve(WtRiy));
MatrixXd theta = theta_tilda * tot_Eta_prec.cwiseInverse().asDiagonal() + theta_star;
return theta;
}