/**
* @function heuristicCost
* @brief
*/
double M_RRT::heuristicCost( Eigen::VectorXd node )
{
Eigen::Transform<double, 3, Eigen::Affine> T;
// Calculate the EE position
robinaLeftArm_fk( node, TWBase, Tee, T );
Eigen::VectorXd trans_ee = T.translation();
Eigen::VectorXd x_ee = T.rotation().col(0);
Eigen::VectorXd y_ee = T.rotation().col(1);
Eigen::VectorXd z_ee = T.rotation().col(2);
Eigen::VectorXd GH = ( goalPosition - trans_ee );
double fx1 = GH.norm() ;
GH = GH/GH.norm();
double fx2 = abs( GH.dot( x_ee ) - 1 );
double fx3 = abs( GH.dot( z_ee ) );
double heuristic = w1*fx1 + w2*fx2 + w3*fx3;
return heuristic;
}
/**
* Returns the expected return of the given portfolio structure.
*
* @param portfolio the portfolio structure
*
* @return the expected portfolio return
*/
double EfficientPortfolioWithRisklessAsset::portfolioReturn(const Eigen::VectorXd& portfolio)
{
Eigen::VectorXd riskyAssets = portfolio.head(dim - 1);
double riskReturn = riskyAssets.dot(mean);
double risklessReturn = portfolio(dim - 1) * _risklessRate;
return riskyAssets.dot(mean) + portfolio(dim - 1) * _risklessRate;
}
bool FilterBase::checkMahalanobisThreshold(const Eigen::VectorXd &innovation,
const Eigen::MatrixXd &invCovariance,
const double nsigmas)
{
double sqMahalanobis = innovation.dot(invCovariance * innovation);
double threshold = nsigmas*nsigmas;
if (sqMahalanobis >= threshold)
{
if (getDebug())
{
*debugStream_ << "Innovation mahalanobis distance test failed. Squared Mahalanobis is\n";
*debugStream_ << sqMahalanobis << "\n";
*debugStream_ << "threshold was:\n";
*debugStream_ << threshold << "\n";
*debugStream_ << "Innovation:\n";
*debugStream_ << innovation << "\n";
*debugStream_ << "Inv covariance:\n";
*debugStream_ << invCovariance << "\n";
}
return false;
}
return true;
}
void constMatrix::gradient_add( const Eigen::VectorXd & X, const Eigen::VectorXd & iV)
{
Eigen::VectorXd vtmp = Q * X;
double xtQx = vtmp.dot(iV.asDiagonal() * vtmp);
dtau += (d - xtQx)/ tau;
ddtau -= (d + xtQx)/ pow(tau, 2);
}
double ObjectiveMLSNoCopy::eval(const Eigen::VectorXd& x) const
{
double obj = 0.0;
for(int i = 0; i < A_->cols(); ++i)
obj -= logsig(-x.dot(A_->col(i)) - b_->coeff(i));
obj /= (double)A_->cols();
return obj;
}
CircularPathSegment(const Eigen::VectorXd &start, const Eigen::VectorXd &intersection, const Eigen::VectorXd &end, double maxDeviation) {
if((intersection - start).norm() < 0.000001 || (end - intersection).norm() < 0.000001) {
length = 0.0;
radius = 1.0;
center = intersection;
x = Eigen::VectorXd::Zero(start.size());
y = Eigen::VectorXd::Zero(start.size());
return;
}
const Eigen::VectorXd startDirection = (intersection - start).normalized();
const Eigen::VectorXd endDirection = (end - intersection).normalized();
if((startDirection - endDirection).norm() < 0.000001) {
length = 0.0;
radius = 1.0;
center = intersection;
x = Eigen::VectorXd::Zero(start.size());
y = Eigen::VectorXd::Zero(start.size());
return;
}
// const double startDistance = (start - intersection).norm();
// const double endDistance = (end - intersection).norm();
double distance = std::min((start - intersection).norm(), (end - intersection).norm());
const double angle = acos(startDirection.dot(endDirection));
distance = std::min(distance, maxDeviation * sin(0.5 * angle) / (1.0 - cos(0.5 * angle))); // enforce max deviation
radius = distance / tan(0.5 * angle);
length = angle * radius;
center = intersection + (endDirection - startDirection).normalized() * radius / cos(0.5 * angle);
x = (intersection - distance * startDirection - center).normalized();
y = startDirection;
//debug
double dotStart = startDirection.dot((intersection - getConfig(0.0)).normalized());
double dotEnd = endDirection.dot((getConfig(length) - intersection).normalized());
if(std::abs(dotStart - 1.0) > 0.0001 || std::abs(dotEnd - 1.0) > 0.0001) {
std::cout << "Error\n";
}
}
Eigen::VectorXd get_symmetric_point(
const Eigen::VectorXd& _normal,
const Eigen::VectorXd& _center,
const Eigen::VectorXd& _point)
{
// Assume that '_normal' is normalized.
Eigen::VectorXd plane_to_point = _normal * _normal.dot(_point - _center);
Eigen::VectorXd symmetric_point = _point - (plane_to_point * 2);
return symmetric_point;
}
double cMathUtil::EvalGaussianLogp(const Eigen::VectorXd& mean, const Eigen::VectorXd& covar, const Eigen::VectorXd& sample)
{
int data_size = static_cast<int>(covar.size());
Eigen::VectorXd diff = sample - mean;
double logp = -0.5 * diff.dot(diff.cwiseQuotient(covar));
double det = covar.prod();
logp += -0.5 * (data_size * std::log(2 * M_PI) + std::log(det));
return logp;
}
double cMathUtil::EvalGaussian(const Eigen::VectorXd& mean, const Eigen::VectorXd& covar, const Eigen::VectorXd& sample)
{
assert(mean.size() == covar.size());
assert(sample.size() == covar.size());
Eigen::VectorXd diff = sample - mean;
double exp_val = diff.dot(diff.cwiseQuotient(covar));
double likelihood = std::exp(-0.5 * exp_val);
double partition = CalcGaussianPartition(covar);
likelihood /= partition;
return likelihood;
}
bool FilterBase::checkMahalanobisThreshold(const Eigen::VectorXd &innovation,
const Eigen::MatrixXd &invCovariance,
const double nsigmas)
{
double sqMahalanobis = innovation.dot(invCovariance * innovation);
double threshold = nsigmas * nsigmas;
if (sqMahalanobis >= threshold)
{
FB_DEBUG("Innovation mahalanobis distance test failed. Squared Mahalanobis is: " << sqMahalanobis << "\n" <<
"Threshold is: " << threshold << "\n" <<
"Innovation is: " << innovation << "\n" <<
"Innovation covariance is:\n" << invCovariance << "\n");
return false;
}
return true;
}
TEST(advi_test, multivar_no_constraint_meanfield) {
// Create mock data_var_context
static const std::string DATA = "";
std::stringstream data_stream(DATA);
stan::io::dump dummy_context(data_stream);
// Instantiate model
Model my_model(dummy_context);
// RNG
rng_t base_rng(0);
// Other params
int n_monte_carlo_grad = 10;
std::stringstream output;
stan::interface_callbacks::writer::stream_writer message_writer(output);
// Dummy input
Eigen::VectorXd cont_params = Eigen::VectorXd::Zero(2);
cont_params(0) = 0.75;
cont_params(1) = 0.75;
// ADVI
stan::variational::advi<Model, stan::variational::normal_meanfield, rng_t>
test_advi(my_model,
cont_params,
base_rng,
n_monte_carlo_grad,
1e4, // absurdly high!
100,
1);
// Create some arbitrary variational q() family to calculate the ELBO over
Eigen::VectorXd mu = Eigen::VectorXd::Constant(my_model.num_params_r(),
2.5);
Eigen::VectorXd sigma_tilde = Eigen::VectorXd::Constant(
my_model.num_params_r(),
0.0); // initializing sigma_tilde = 0
// means sigma = 1
stan::variational::normal_meanfield musigmatilde =
stan::variational::normal_meanfield(mu, sigma_tilde);
double elbo = 0.0;
elbo = test_advi.calc_ELBO(musigmatilde, message_writer);
// Can calculate ELBO analytically
double zeta = -0.5 * ( 3*2*log(2.0*stan::math::pi()) + 18.5 + 25 + 13 );
Eigen::VectorXd mu_J = Eigen::VectorXd::Zero(2);
mu_J(0) = 10.5;
mu_J(1) = 7.5;
double elbo_true = 0.0;
elbo_true += zeta;
elbo_true += mu_J.dot(mu);
elbo_true += -0.5 * ( 3*mu.dot(mu) + 3*2 );
elbo_true += 1 + log(2.0*stan::math::pi());
double const EPSILON = 0.1;
EXPECT_NEAR(elbo_true, elbo, EPSILON);
Eigen::VectorXd mu_grad = Eigen::VectorXd::Zero(3);
Eigen::VectorXd st_grad = Eigen::VectorXd::Zero(my_model.num_params_r());
std::string error = "stan::variational::normal_meanfield: "
"Dimension of mean vector (3) and "
"Dimension of log std vector (2) must match in size";
EXPECT_THROW_MSG(stan::variational::normal_meanfield(mu_grad, st_grad),
std::invalid_argument, error);
mu_grad = Eigen::VectorXd::Zero(0);
error = "stan::variational::normal_meanfield: "
"Dimension of mean vector (0) and "
"Dimension of log std vector (2) must match in size";
EXPECT_THROW_MSG(stan::variational::normal_meanfield(mu_grad, st_grad),
std::invalid_argument, error);
mu_grad = Eigen::VectorXd::Zero(my_model.num_params_r());
st_grad = Eigen::VectorXd::Zero(3);
error = "stan::variational::normal_meanfield: "
"Dimension of mean vector (2) and "
"Dimension of log std vector (3) must match in size";
EXPECT_THROW_MSG(stan::variational::normal_meanfield(mu_grad, st_grad),
std::invalid_argument, error);
mu_grad = Eigen::VectorXd::Zero(my_model.num_params_r());
st_grad = Eigen::VectorXd::Zero(0);
error = "stan::variational::normal_meanfield: "
"Dimension of mean vector (2) and "
"Dimension of log std vector (0) must match in size";
EXPECT_THROW_MSG(stan::variational::normal_meanfield(mu_grad, st_grad),
std::invalid_argument, error);
mu_grad = Eigen::VectorXd::Zero(3);
st_grad = Eigen::VectorXd::Zero(3);
stan::variational::normal_meanfield elbo_grad = stan::variational::normal_meanfield(mu_grad, st_grad);
error = "stan::variational::normal_meanfield::calc_grad: "
"Dimension of elbo_grad (3) and "
"Dimension of variational q (2) must match in size";
EXPECT_THROW_MSG(musigmatilde.calc_grad(elbo_grad,
my_model, cont_params, n_monte_carlo_grad,
base_rng, message_writer),
std::invalid_argument, error);
}
TEST(advi_test, multivar_no_constraint_fullrank) {
// Create mock data_var_context
static const std::string DATA = "";
std::stringstream data_stream(DATA);
stan::io::dump dummy_context(data_stream);
// Instantiate model
Model my_model(dummy_context);
// RNG
rng_t base_rng(0);
// Other params
int n_monte_carlo_grad = 10;
int n_grad_samples = 1e4;
std::stringstream output;
stan::interface_callbacks::writer::stream_writer message_writer(output);
// Dummy input
Eigen::VectorXd cont_params = Eigen::VectorXd::Zero(2);
cont_params(0) = 0.75;
cont_params(1) = 0.75;
// ADVI
stan::variational::advi<Model, stan::variational::normal_fullrank, rng_t>
test_advi(my_model,
cont_params,
base_rng,
n_monte_carlo_grad,
n_grad_samples,
100,
1);
// Create some arbitrary variational q() family to calculate the ELBO over
Eigen::VectorXd mu = Eigen::VectorXd::Constant(my_model.num_params_r(),
2.5);
Eigen::MatrixXd L_chol = Eigen::MatrixXd::Identity(my_model.num_params_r(),
my_model.num_params_r());
stan::variational::normal_fullrank muL =
stan::variational::normal_fullrank(mu, L_chol);
double elbo = 0.0;
elbo = test_advi.calc_ELBO(muL, message_writer);
// Can calculate ELBO analytically
double zeta = -0.5 * ( 3*2*log(2.0*stan::math::pi()) + 18.5 + 25 + 13 );
Eigen::VectorXd mu_J = Eigen::VectorXd::Zero(2);
mu_J(0) = 10.5;
mu_J(1) = 7.5;
double elbo_true = 0.0;
elbo_true += zeta;
elbo_true += mu_J.dot(mu);
elbo_true += -0.5 * ( 3*mu.dot(mu) + 3*2 );
elbo_true += 1 + log(2.0*stan::math::pi());
double const EPSILON = 0.1;
EXPECT_NEAR(elbo_true, elbo, EPSILON);
Eigen::VectorXd mu_grad = Eigen::VectorXd::Zero(3);
Eigen::MatrixXd L_grad = Eigen::MatrixXd::Identity(my_model.num_params_r(),
my_model.num_params_r());
std::string error = "stan::variational::normal_fullrank: "
"Dimension of mean vector (3) and "
"Dimension of Cholesky factor (2) must match in size";
EXPECT_THROW_MSG(stan::variational::normal_fullrank(mu_grad, L_grad),
std::invalid_argument, error);
mu_grad = Eigen::VectorXd::Zero(0);
error = "stan::variational::normal_fullrank: "
"Dimension of mean vector (0) and "
"Dimension of Cholesky factor (2) must match in size";
EXPECT_THROW_MSG(stan::variational::normal_fullrank(mu_grad, L_grad),
std::invalid_argument, error);
mu_grad = Eigen::VectorXd::Zero(my_model.num_params_r());
L_grad = Eigen::MatrixXd::Identity(3,3);
error = "stan::variational::normal_fullrank: "
"Dimension of mean vector (2) and "
"Dimension of Cholesky factor (3) must match in size";
EXPECT_THROW_MSG(stan::variational::normal_fullrank(mu_grad, L_grad),
std::invalid_argument, error);
mu_grad = Eigen::VectorXd::Zero(my_model.num_params_r());
L_grad = Eigen::MatrixXd::Identity(0,0);
error = "stan::variational::normal_fullrank: "
"Dimension of mean vector (2) and "
"Dimension of Cholesky factor (0) must match in size";
EXPECT_THROW_MSG(stan::variational::normal_fullrank(mu_grad, L_grad),
std::invalid_argument, error);
mu_grad = Eigen::VectorXd::Zero(my_model.num_params_r());
L_grad = Eigen::MatrixXd::Identity(1,4);
error = "stan::variational::normal_fullrank: "
"Expecting a square matrix; rows of Cholesky factor (1) and columns "
"of Cholesky factor (4) must match in size";
EXPECT_THROW_MSG(stan::variational::normal_fullrank(mu_grad, L_grad),
std::invalid_argument, error);
mu_grad = Eigen::VectorXd::Zero(3);
L_grad = Eigen::MatrixXd::Identity(3,3);
stan::variational::normal_fullrank elbo_grad = stan::variational::normal_fullrank(mu_grad, L_grad);
error = "stan::variational::normal_fullrank::calc_grad: "
"Dimension of elbo_grad (3) and "
"Dimension of variational q (2) must match in size";
EXPECT_THROW_MSG(muL.calc_grad(elbo_grad,
my_model, cont_params, n_monte_carlo_grad,
base_rng, message_writer),
std::invalid_argument, error);
}
IGL_INLINE bool igl::copyleft::quadprog(
const Eigen::MatrixXd & G,
const Eigen::VectorXd & g0,
const Eigen::MatrixXd & CE,
const Eigen::VectorXd & ce0,
const Eigen::MatrixXd & CI,
const Eigen::VectorXd & ci0,
Eigen::VectorXd& x)
{
using namespace Eigen;
typedef double Scalar;
const auto distance = [](Scalar a, Scalar b)->Scalar
{
Scalar a1, b1, t;
a1 = std::abs(a);
b1 = std::abs(b);
if (a1 > b1)
{
t = (b1 / a1);
return a1 * std::sqrt(1.0 + t * t);
}
else
if (b1 > a1)
{
t = (a1 / b1);
return b1 * std::sqrt(1.0 + t * t);
}
return a1 * std::sqrt(2.0);
};
const auto compute_d = [](VectorXd &d, const MatrixXd& J, const VectorXd& np)
{
d = J.adjoint() * np;
};
const auto update_z =
[](VectorXd& z, const MatrixXd& J, const VectorXd& d, int iq)
{
z = J.rightCols(z.size()-iq) * d.tail(d.size()-iq);
};
const auto update_r =
[](const MatrixXd& R, VectorXd& r, const VectorXd& d, int iq)
{
r.head(iq) =
R.topLeftCorner(iq,iq).triangularView<Upper>().solve(d.head(iq));
};
const auto add_constraint = [&distance](
MatrixXd& R,
MatrixXd& J,
VectorXd& d,
int& iq,
double& R_norm)->bool
{
int n=J.rows();
#ifdef TRACE_SOLVER
std::cerr << "Add constraint " << iq << '/';
#endif
int i, j, k;
double cc, ss, h, t1, t2, xny;
/* we have to find the Givens rotation which will reduce the element
d(j) to zero.
if it is already zero we don't have to do anything, except of
decreasing j */
for (j = n - 1; j >= iq + 1; j--)
{
/* The Givens rotation is done with the matrix (cc cs, cs -cc).
If cc is one, then element (j) of d is zero compared with element
(j - 1). Hence we don't have to do anything.
If cc is zero, then we just have to switch column (j) and column (j - 1)
of J. Since we only switch columns in J, we have to be careful how we
update d depending on the sign of gs.
Otherwise we have to apply the Givens rotation to these columns.
The i - 1 element of d has to be updated to h. */
cc = d(j - 1);
ss = d(j);
h = distance(cc, ss);
if (h == 0.0)
continue;
d(j) = 0.0;
ss = ss / h;
cc = cc / h;
if (cc < 0.0)
{
cc = -cc;
ss = -ss;
d(j - 1) = -h;
}
else
d(j - 1) = h;
xny = ss / (1.0 + cc);
for (k = 0; k < n; k++)
{
t1 = J(k,j - 1);
t2 = J(k,j);
J(k,j - 1) = t1 * cc + t2 * ss;
J(k,j) = xny * (t1 + J(k,j - 1)) - t2;
}
}
/* update the number of constraints added*/
iq++;
/* To update R we have to put the iq components of the d vector
into column iq - 1 of R
*/
R.col(iq-1).head(iq) = d.head(iq);
#ifdef TRACE_SOLVER
std::cerr << iq << std::endl;
#endif
if (std::abs(d(iq - 1)) <= std::numeric_limits<double>::epsilon() * R_norm)
{
// problem degenerate
return false;
}
R_norm = std::max<double>(R_norm, std::abs(d(iq - 1)));
return true;
};
const auto delete_constraint = [&distance](
MatrixXd& R,
MatrixXd& J,
VectorXi& A,
VectorXd& u,
int p,
int& iq,
int l)
{
int n = R.rows();
#ifdef TRACE_SOLVER
std::cerr << "Delete constraint " << l << ' ' << iq;
#endif
int i, j, k, qq;
double cc, ss, h, xny, t1, t2;
/* Find the index qq for active constraint l to be removed */
for (i = p; i < iq; i++)
if (A(i) == l)
{
qq = i;
break;
}
/* remove the constraint from the active set and the duals */
for (i = qq; i < iq - 1; i++)
{
A(i) = A(i + 1);
u(i) = u(i + 1);
R.col(i) = R.col(i+1);
}
A(iq - 1) = A(iq);
u(iq - 1) = u(iq);
A(iq) = 0;
u(iq) = 0.0;
for (j = 0; j < iq; j++)
R(j,iq - 1) = 0.0;
/* constraint has been fully removed */
iq--;
#ifdef TRACE_SOLVER
std::cerr << '/' << iq << std::endl;
#endif
if (iq == 0)
return;
for (j = qq; j < iq; j++)
{
cc = R(j,j);
ss = R(j + 1,j);
h = distance(cc, ss);
if (h == 0.0)
continue;
cc = cc / h;
ss = ss / h;
R(j + 1,j) = 0.0;
if (cc < 0.0)
{
R(j,j) = -h;
cc = -cc;
ss = -ss;
}
else
R(j,j) = h;
xny = ss / (1.0 + cc);
for (k = j + 1; k < iq; k++)
{
t1 = R(j,k);
t2 = R(j + 1,k);
R(j,k) = t1 * cc + t2 * ss;
R(j + 1,k) = xny * (t1 + R(j,k)) - t2;
}
for (k = 0; k < n; k++)
{
t1 = J(k,j);
t2 = J(k,j + 1);
J(k,j) = t1 * cc + t2 * ss;
J(k,j + 1) = xny * (J(k,j) + t1) - t2;
}
}
};
int i, j, k, l; /* indices */
int ip, me, mi;
int n=g0.size(); int p=ce0.size(); int m=ci0.size();
MatrixXd R(G.rows(),G.cols()), J(G.rows(),G.cols());
LLT<MatrixXd,Lower> chol(G.cols());
VectorXd s(m+p), z(n), r(m + p), d(n), np(n), u(m + p);
VectorXd x_old(n), u_old(m + p);
double f_value, psi, c1, c2, sum, ss, R_norm;
const double inf = std::numeric_limits<double>::infinity();
double t, t1, t2; /* t is the step length, which is the minimum of the partial step length t1
* and the full step length t2 */
VectorXi A(m + p), A_old(m + p), iai(m + p);
int q;
int iq, iter = 0;
std::vector<bool> iaexcl(m + p);
me = p; /* number of equality constraints */
mi = m; /* number of inequality constraints */
q = 0; /* size of the active set A (containing the indices of the active constraints) */
/*
* Preprocessing phase
*/
/* compute the trace of the original matrix G */
c1 = G.trace();
/* decompose the matrix G in the form LL^T */
chol.compute(G);
/* initialize the matrix R */
d.setZero();
R.setZero();
R_norm = 1.0; /* this variable will hold the norm of the matrix R */
/* compute the inverse of the factorized matrix G^-1, this is the initial value for H */
// J = L^-T
J.setIdentity();
J = chol.matrixU().solve(J);
c2 = J.trace();
#ifdef TRACE_SOLVER
print_matrix("J", J, n);
#endif
/* c1 * c2 is an estimate for cond(G) */
/*
* Find the unconstrained minimizer of the quadratic form 0.5 * x G x + g0 x
* this is a feasible point in the dual space
* x = G^-1 * g0
*/
x = chol.solve(g0);
x = -x;
/* and compute the current solution value */
f_value = 0.5 * g0.dot(x);
#ifdef TRACE_SOLVER
std::cerr << "Unconstrained solution: " << f_value << std::endl;
print_vector("x", x, n);
#endif
/* Add equality constraints to the working set A */
iq = 0;
for (i = 0; i < me; i++)
{
np = CE.col(i);
compute_d(d, J, np);
update_z(z, J, d, iq);
update_r(R, r, d, iq);
#ifdef TRACE_SOLVER
print_matrix("R", R, iq);
print_vector("z", z, n);
print_vector("r", r, iq);
print_vector("d", d, n);
#endif
/* compute full step length t2: i.e., the minimum step in primal space s.t. the contraint
becomes feasible */
t2 = 0.0;
if (std::abs(z.dot(z)) > std::numeric_limits<double>::epsilon()) // i.e. z != 0
t2 = (-np.dot(x) - ce0(i)) / z.dot(np);
x += t2 * z;
/* set u = u+ */
u(iq) = t2;
u.head(iq) -= t2 * r.head(iq);
/* compute the new solution value */
f_value += 0.5 * (t2 * t2) * z.dot(np);
A(i) = -i - 1;
if (!add_constraint(R, J, d, iq, R_norm))
{
// FIXME: it should raise an error
// Equality constraints are linearly dependent
return false;
}
}
/* set iai = K \ A */
for (i = 0; i < mi; i++)
iai(i) = i;
l1: iter++;
#ifdef TRACE_SOLVER
print_vector("x", x, n);
#endif
/* step 1: choose a violated constraint */
for (i = me; i < iq; i++)
{
ip = A(i);
iai(ip) = -1;
}
/* compute s(x) = ci^T * x + ci0 for all elements of K \ A */
ss = 0.0;
psi = 0.0; /* this value will contain the sum of all infeasibilities */
ip = 0; /* ip will be the index of the chosen violated constraint */
for (i = 0; i < mi; i++)
{
iaexcl[i] = true;
sum = CI.col(i).dot(x) + ci0(i);
s(i) = sum;
psi += std::min(0.0, sum);
}
#ifdef TRACE_SOLVER
print_vector("s", s, mi);
#endif
if (std::abs(psi) <= mi * std::numeric_limits<double>::epsilon() * c1 * c2* 100.0)
{
/* numerically there are not infeasibilities anymore */
q = iq;
return true;
}
/* save old values for u, x and A */
u_old.head(iq) = u.head(iq);
A_old.head(iq) = A.head(iq);
x_old = x;
l2: /* Step 2: check for feasibility and determine a new S-pair */
for (i = 0; i < mi; i++)
{
if (s(i) < ss && iai(i) != -1 && iaexcl[i])
{
ss = s(i);
ip = i;
}
}
if (ss >= 0.0)
{
q = iq;
return true;
}
/* set np = n(ip) */
np = CI.col(ip);
/* set u = (u 0)^T */
u(iq) = 0.0;
/* add ip to the active set A */
A(iq) = ip;
#ifdef TRACE_SOLVER
std::cerr << "Trying with constraint " << ip << std::endl;
print_vector("np", np, n);
#endif
l2a:/* Step 2a: determine step direction */
/* compute z = H np: the step direction in the primal space (through J, see the paper) */
compute_d(d, J, np);
update_z(z, J, d, iq);
/* compute N* np (if q > 0): the negative of the step direction in the dual space */
update_r(R, r, d, iq);
#ifdef TRACE_SOLVER
std::cerr << "Step direction z" << std::endl;
print_vector("z", z, n);
print_vector("r", r, iq + 1);
print_vector("u", u, iq + 1);
print_vector("d", d, n);
print_ivector("A", A, iq + 1);
#endif
/* Step 2b: compute step length */
l = 0;
/* Compute t1: partial step length (maximum step in dual space without violating dual feasibility */
t1 = inf; /* +inf */
/* find the index l s.t. it reaches the minimum of u+(x) / r */
for (k = me; k < iq; k++)
{
double tmp;
if (r(k) > 0.0 && ((tmp = u(k) / r(k)) < t1) )
{
t1 = tmp;
l = A(k);
}
}
/* Compute t2: full step length (minimum step in primal space such that the constraint ip becomes feasible */
if (std::abs(z.dot(z)) > std::numeric_limits<double>::epsilon()) // i.e. z != 0
t2 = -s(ip) / z.dot(np);
else
t2 = inf; /* +inf */
/* the step is chosen as the minimum of t1 and t2 */
t = std::min(t1, t2);
#ifdef TRACE_SOLVER
std::cerr << "Step sizes: " << t << " (t1 = " << t1 << ", t2 = " << t2 << ") ";
#endif
/* Step 2c: determine new S-pair and take step: */
/* case (i): no step in primal or dual space */
if (t >= inf)
{
/* QPP is infeasible */
// FIXME: unbounded to raise
q = iq;
return false;
}
/* case (ii): step in dual space */
if (t2 >= inf)
{
/* set u = u + t * [-r 1) and drop constraint l from the active set A */
u.head(iq) -= t * r.head(iq);
u(iq) += t;
iai(l) = l;
delete_constraint(R, J, A, u, p, iq, l);
#ifdef TRACE_SOLVER
std::cerr << " in dual space: "
<< f_value << std::endl;
print_vector("x", x, n);
print_vector("z", z, n);
print_ivector("A", A, iq + 1);
#endif
goto l2a;
}
/* case (iii): step in primal and dual space */
x += t * z;
/* update the solution value */
f_value += t * z.dot(np) * (0.5 * t + u(iq));
u.head(iq) -= t * r.head(iq);
u(iq) += t;
#ifdef TRACE_SOLVER
std::cerr << " in both spaces: "
<< f_value << std::endl;
print_vector("x", x, n);
print_vector("u", u, iq + 1);
print_vector("r", r, iq + 1);
print_ivector("A", A, iq + 1);
#endif
if (t == t2)
{
#ifdef TRACE_SOLVER
std::cerr << "Full step has taken " << t << std::endl;
print_vector("x", x, n);
#endif
/* full step has taken */
/* add constraint ip to the active set*/
if (!add_constraint(R, J, d, iq, R_norm))
{
iaexcl[ip] = false;
delete_constraint(R, J, A, u, p, iq, ip);
#ifdef TRACE_SOLVER
print_matrix("R", R, n);
print_ivector("A", A, iq);
#endif
for (i = 0; i < m; i++)
iai(i) = i;
for (i = 0; i < iq; i++)
{
A(i) = A_old(i);
iai(A(i)) = -1;
u(i) = u_old(i);
}
x = x_old;
goto l2; /* go to step 2 */
}
else
iai(ip) = -1;
#ifdef TRACE_SOLVER
print_matrix("R", R, n);
print_ivector("A", A, iq);
#endif
goto l1;
}
/* a patial step has taken */
#ifdef TRACE_SOLVER
std::cerr << "Partial step has taken " << t << std::endl;
print_vector("x", x, n);
#endif
/* drop constraint l */
iai(l) = l;
delete_constraint(R, J, A, u, p, iq, l);
#ifdef TRACE_SOLVER
print_matrix("R", R, n);
print_ivector("A", A, iq);
#endif
s(ip) = CI.col(ip).dot(x) + ci0(ip);
#ifdef TRACE_SOLVER
print_vector("s", s, mi);
#endif
goto l2a;
}
static void
_solve_linear_system(TriMesh& src, const TriMesh& dst,
const std::vector<std::vector<int>>& tri_neighbours,
const std::vector<std::pair<int, int> >& corres,
double weights[3]
) {
int rows = 0; int cols = src.vert_num + src.poly_num - corres.size();
assert (tri_neighbours.size() == src.poly_num);
std::vector<std::pair<int, int>> soft_corres;
_setup_kd_correspondence(src, dst, soft_corres);
#if 0
std::ofstream ofs("E:/data/ScapeOriginal/build/data_ming/dt_pairs.txt");
ofs << soft_corres.size() << endl;
for (size_t i=0; i<soft_corres.size(); ++i)
ofs << soft_corres[i].first << "\t" << soft_corres[i].second << std::endl;
printf("check soft pair\n");
getchar();
#endif
for (int i=0; i<src.poly_num; ++i)
rows += 3*tri_neighbours[i].size(); //smooth part
rows += src.poly_num*3; //identity part
static std::vector<bool> vertex_flag; //indicate whether the vertex is hard constrainted by corres
if (vertex_flag.empty()) {
vertex_flag.resize(src.vert_num, false);
for (size_t i=0; i<corres.size(); ++i)
vertex_flag[corres[i].first] = true;
}
for (int i=0; i<soft_corres.size(); ++i) { //soft constraints part
if (vertex_flag[soft_corres[i].first]) continue;
++rows;
}
//vertex_real_col stores two information : unknow vertex's col(offset by
//poly_num) in X and know vertexs' corresponding index in dst mesh.
//there is no need to compute this in each iteration
static std::vector<int> vertex_real_col;
if (vertex_real_col.empty()) {
vertex_real_col.resize(src.vert_num, -1);
std::map<int, int> corres_map;
for (int i=0; i<corres.size(); ++i) corres_map.insert(std::make_pair(corres[i].first, corres[i].second));
int real_col = 0;
for (int i=0; i<src.vert_num; ++i) {
if (vertex_flag[i]) {
vertex_real_col[i] = corres_map[i];
} else {
vertex_real_col[i] = real_col;
real_col++;
}
}
assert (real_col == src.vert_num - corres.size()); //make sure indexes in corres are different from each other
}
SparseMatrix<double> A(rows, cols);
A.reserve(Eigen::VectorXi::Constant(cols, 200));
std::vector<VectorXd> Y(3, VectorXd(rows));
Y[0].setZero();Y[1].setZero();Y[2].setZero();
//precompute Q_hat^-1 in Q_hat_inverse [n v2-v1 v3-v1]^-1
src.updateNorm();
assert (!src.face_norm.empty());
std::vector<Matrix3d> Q_hat_inverse(src.poly_num);
Eigen::Matrix3d _inverse;
for (int i=0; i<src.poly_num; ++i) {
unsigned int* index_i = src.polyIndex[i].vert_index;
Vector3d v[3];
v[0] = src.face_norm[i];
v[1] = src.vertex_coord[index_i[1]] - src.vertex_coord[index_i[0]];//v2-v1
v[2] = src.vertex_coord[index_i[2]] - src.vertex_coord[index_i[0]];//v3-v1
for (int k=0; k<3; ++k)
for (int j=0; j<3; ++j)
Q_hat_inverse[i](k, j) = v[j][k];
_inverse = Q_hat_inverse[i].inverse();
Q_hat_inverse[i] = _inverse;
}
int energy_size[3] = {0, 0, 0}; //each energy part's starting index
//start establishing the large linear sparse system
double weight_smooth = weights[0];
int row = 0;
for (int i=0; i<src.poly_num; ++i) {
Eigen::Matrix3d& Q_i_hat = Q_hat_inverse[i];
unsigned int* index_i = src.polyIndex[i].vert_index;
for (size_t _j=0; _j<tri_neighbours[i].size(); ++_j) {
int j = tri_neighbours[i][_j]; //triangle index
Eigen::Matrix3d& Q_j_hat = Q_hat_inverse[j];
unsigned int* index_j = src.polyIndex[j].vert_index;
for (int k=0; k<3; ++k)
for (int dim = 0; dim < 3; ++dim)
Y[dim](row+k) = 0.0;
for (int k=0; k<3; ++k) {
A.coeffRef(row+k, i) = weight_smooth*Q_i_hat(0, k); //n
A.coeffRef(row+k, j) = -weight_smooth*Q_j_hat(0, k); //n
}
for (int k=0; k<3; ++k)
for (int p=0; p<3; ++p)
if (!vertex_flag[index_i[p]])
A.coeffRef(row+k, src.poly_num+vertex_real_col[index_i[p]]) = 0.0;
if (vertex_flag[index_i[0]]) {
for (int k=0; k<3; ++k) {
for (int dim = 0; dim < 3; ++dim)
Y[dim](row + k) += weight_smooth*(Q_i_hat(1, k)+Q_i_hat(2, k))*dst.vertex_coord[vertex_real_col[index_i[0]]][dim];
}
} else {
for (int k=0; k<3; ++k)
A.coeffRef(row+k, src.poly_num + vertex_real_col[index_i[0]]) +=
-weight_smooth*(Q_i_hat(1, k)+Q_i_hat(2, k));
}
if (vertex_flag[index_j[0]]) {
for (int k=0; k<3; ++k) {
for (int dim = 0; dim < 3; ++dim)
Y[dim](row + k) += -weight_smooth*(Q_j_hat(1, k)+Q_j_hat(2, k))*dst.vertex_coord[vertex_real_col[index_j[0]]][dim];
}
} else {
for (int k=0; k<3; ++k)
A.coeffRef(row+k, src.poly_num + vertex_real_col[index_j[0]]) +=
weight_smooth*(Q_j_hat(1, k)+Q_j_hat(2, k));
}
for (int p=1; p<3; ++p) {//v2 or v3
if (vertex_flag[index_i[p]]) {
for (int k=0; k<3; ++k)
for (int dim=0; dim<3; ++dim)
Y[dim](row + k) += -weight_smooth*Q_i_hat(p, k)*dst.vertex_coord[vertex_real_col[index_i[p]]][dim];
} else {
for (int k=0; k<3; ++k)
A.coeffRef(row+k, src.poly_num + vertex_real_col[index_i[p]]) += weight_smooth*Q_i_hat(p, k);
}
}
for (int p=1; p<3; ++p) {
if (vertex_flag[index_j[p]]) {
for (int k=0; k<3; ++k)
for (int dim=0; dim < 3; ++dim)
Y[dim](row + k) += weight_smooth*Q_j_hat(p, k)*dst.vertex_coord[vertex_real_col[index_j[p]]][dim];
} else {
for (int k=0; k<3; ++k)
A.coeffRef(row+k, src.poly_num + vertex_real_col[index_j[p]]) += -weight_smooth*Q_j_hat(p, k);
}
}
row += 3;
}
}
energy_size[0] = row;
double weight_identity = weights[1];
for (int i=0; i<src.poly_num; ++i) {
Eigen::Matrix3d& Q_i_hat = Q_hat_inverse[i];
unsigned int* index_i = src.polyIndex[i].vert_index;
Y[0](row) = weight_identity; Y[0](row+1) = 0.0; Y[0](row+2) = 0.0;
Y[1](row) = 0.0; Y[1](row+1) = weight_identity; Y[1](row+2) = 0.0;
Y[2](row) = 0.0; Y[2](row+1) = 0.0; Y[2](row+2) = weight_identity;
for (int k=0; k<3; ++k)
A.coeffRef(row+k, i) = weight_identity*Q_i_hat(0, k); //n
if (vertex_flag[index_i[0]]) {
for (int k=0; k<3; ++k)
for (int dim = 0; dim < 3; ++dim)
Y[dim](row+k) += weight_identity*(Q_i_hat(1, k)+Q_i_hat(2,k))*dst.vertex_coord[vertex_real_col[index_i[0]]][dim];
} else {
for (int k=0; k<3; ++k)
A.coeffRef(row+k, src.poly_num+vertex_real_col[index_i[0]]) = -weight_identity*(Q_i_hat(1, k)+Q_i_hat(2,k));
}
for (int p=1; p<3; ++p) {
if (vertex_flag[index_i[p]]) {
for (int k=0; k<3; ++k)
for (int dim=0; dim<3; ++dim)
Y[dim](row + k) += -weight_identity*Q_i_hat(p, k)*dst.vertex_coord[vertex_real_col[index_i[p]]][dim];
} else {
for (int k=0; k<3; ++k)
A.coeffRef(row+k, src.poly_num + vertex_real_col[index_i[p]]) = weight_identity*Q_i_hat(p, k);
}
}
row += 3;
}
energy_size[1] = row;
double weight_soft_constraint = weights[2];
for (int i=0; i<soft_corres.size(); ++i) {
if (vertex_flag[soft_corres[i].first]) continue;
A.coeffRef(row, src.poly_num + vertex_real_col[soft_corres[i].first]) = weight_soft_constraint;
for (int dim=0; dim<3; ++dim)
Y[dim](row) += weight_soft_constraint*dst.vertex_coord[soft_corres[i].second][dim];
++row;
}
energy_size[2] = row;
assert (row == rows);
//start solving the least-square problem
fprintf(stdout, "finished filling matrix\n");
Eigen::SparseMatrix<double> At = A.transpose();
Eigen::SparseMatrix<double> AtA = At*A;
Eigen::SimplicialCholesky<SparseMatrix<double>> solver;
solver.compute(AtA);
if (solver.info() != Eigen::Success) {
fprintf(stdout, "unable to defactorize AtA\n");
exit(-1);
}
VectorXd X[3];
for (int i=0; i<3; ++i) {
VectorXd AtY = At*Y[i];
X[i] = solver.solve(AtY);
Eigen::VectorXd Energy = A*X[i] - Y[i];
Eigen::VectorXd smoothEnergy = Energy.head(energy_size[0]);
Eigen::VectorXd identityEnergy = Energy.segment(energy_size[0], energy_size[1]-energy_size[0]);
Eigen::VectorXd softRegularEnergy = Energy.tail(energy_size[2]-energy_size[1]);
fprintf(stdout, "\t%lf = %lf + %lf + %lf\n",
Energy.dot(Energy), smoothEnergy.dot(smoothEnergy),
identityEnergy.dot(identityEnergy), softRegularEnergy.dot(softRegularEnergy));
}
//fill data back to src
for (int i=0; i<src.poly_num; ++i)
for (int d=0; d<3; ++d)
src.face_norm[i][d] = X[d](i);
for (size_t i=0; i<corres.size(); ++i) src.vertex_coord[corres[i].first] = dst.vertex_coord[corres[i].second];
int p = 0;
for (int i=0; i<src.vert_num; ++i) {
if (vertex_flag[i]) continue;
for (int d=0; d<3; ++d)
src.vertex_coord[i][d] = X[d](src.poly_num+p);
++p;
}
return;
}
/**
* Returns the variance of the given portfolio structure.
*
* @param portfolio the portfolio structure
*
* @return the portfolio variance
*/
double EfficientPortfolioWithRisklessAsset::portfolioVariance(const Eigen::VectorXd& portfolio)
{
Eigen::VectorXd riskyAssets = portfolio.head(dim - 1);
return riskyAssets.dot(variance * riskyAssets);
}
double Statistics::scalarProduct(const Eigen::VectorXd& cv)
{
return cv.dot(cv);
}
double Constraint::evalObj() {
Eigen::VectorXd constr = evalCon();
return 0.5 * constr.dot(constr);
/* return 0.5*dot(constr, constr); */
}
