Пример #1
0
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;
}
Пример #3
0
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;
}