MatrixXd infer(const VectorXd& input)
{
auto sigmod = [&](double& elem)
{
return 1/(1 + exp(-elem));
};
MatrixXd output = input.transpose();
for(auto layer=0; layer<weights.size(); ++layer)
{
output = output * weights[layer];
for(auto i=0; i<output.size(); ++i)
{
output(i) = sigmod(output(i));
}
}
return output;
}
void UKF::PredictMeasurement(int n_z, const MatrixXd &Zsig, VectorXd &z_pred, MatrixXd &S, MatrixXd &R) {
// predict measurement mean
z_pred.fill(0.0);
for (int i=0; i < 2 * n_aug_ + 1; i++) {
z_pred += weights_(i) * Zsig.col(i);
}
// measurement covariance matrix S
S.fill(0.0);
for (int i = 0; i < 2 * n_aug_ + 1; i++) {
VectorXd z_diff = Zsig.col(i) - z_pred;
while (z_diff(1) > M_PI) z_diff(1) -= 2. * M_PI;
while (z_diff(1) < -M_PI) z_diff(1) += 2. * M_PI;
S += weights_(i) * z_diff * z_diff.transpose();
}
S += R;
}
int contactPhi(const RigidBodyTree<double>& r,
const KinematicsCache<double>& cache,
// TODO(#2274) Fix NOLINTNEXTLINE(runtime/references).
SupportStateElement& supp, VectorXd& phi) {
int nc = static_cast<int>(supp.contact_pts.size());
phi.resize(nc);
if (nc < 1) return nc;
int i = 0;
for (auto pt_iter = supp.contact_pts.begin();
pt_iter != supp.contact_pts.end(); pt_iter++) {
Vector3d contact_pos = r.transformPoints(cache, *pt_iter, supp.body_idx, 0);
phi(i) = supp.support_surface.head<3>().dot(contact_pos) +
supp.support_surface(3);
i++;
}
return nc;
}
double Ebend(const vector<vector<int>> &springpairs,
const vector<spring> &springlist,
const VectorXd &XY,
const double g11,
const double g12,
const double g22,
const double kappa)
{
double Energy=0;
int num=XY.size()/2;
double l1,l2,costh;
double x1,y1,x21,y21,x23,y23,x3,y3;
for(int i=0;i<springpairs.size();i++){
int springone=springpairs[i][0];
int springtwo=springpairs[i][1];
int coordNRone=springlist[springone].one;
int coordNRtwo=springlist[springone].two;
int coordNRthree=springlist[springtwo].two;
x1=XY(coordNRone);
y1=XY(coordNRone+num);
x21=XY(coordNRtwo)+springlist[springone].wlr; //version of (x2,y2) that lies in on spring 1, so possibly outside of the box
y21=XY(coordNRtwo+num)+springlist[springone].wud;
x23=XY(coordNRtwo); //version of (x2,y2) that is on spring 2, so MUST be inside the box
y23=XY(coordNRtwo+num);
x3=XY(coordNRthree)+springlist[springtwo].wlr;
y3=XY(coordNRthree+num)+springlist[springtwo].wud;
l1=Dist(x1,y1,x21,y21,g11,g12,g22);
l2=Dist(x23,y23,x3,y3,g11,g12,g22);
costh=(g11*(x21-x1)*(x3-x23)+g12*(x21-x1)*(y3-y23)+g12*(y21-y1)*(x3-x23)+g22*(y21-y1)*(y3-y23))/(l1*l2);
if(costh>1.0) costh=1.0;
if(costh<-1.0) costh=-1.0;
Energy=Energy+kappa*pow(pi-acos(costh),2)/(l1+l2);
}
return Energy;
}
SparseMatrix<double> spmtimesd(const SparseMatrix<double>& m, const VectorXd& d1, const VectorXd& d2)
{
assert(m.rows() == d1.rows() || d1.rows() == 0);
assert(m.cols() == d2.rows() || d2.rows() == 0);
SparseMatrix<double> result(m.rows(), m.cols());
result = m;
if(d1.rows() > 0)
result = sparseFromDiag(d1) * result;
if(d2.rows() > 0)
result = result * sparseFromDiag(d2);
return result;
}
void CGppe::Make_Predictions_New_User(const VectorXd & theta_x, const VectorXd& theta_t, double& sigma, const MatrixXd& train_t, const MatrixXd &x, const TypePair & train_pairs,
const VectorXd & idx_global, const VectorXd& idx_global_1, const VectorXd& idx_global_2,
const VectorXd& ind_t, const VectorXd& ind_x, const MatrixXd & test_t, const MatrixXd& idx_pairs, const VectorXd& ftrue, const VectorXd& ytrue)
{
int N = x.rows();
int Mtrain = train_t.rows();
int Npairs = idx_pairs.rows();
VectorXd fstar;
MatrixXd pair;
VectorXd P = VectorXd::Zero(Npairs);
VectorXd ypred = VectorXd::Zero(Npairs);
VectorXd sum = VectorXd::Zero(N);
VectorXd count = VectorXd::Zero(N);
Approx_CGppe_Laplace( theta_x, theta_t, sigma,
train_t, x, train_pairs, idx_global, idx_global_1, idx_global_2, ind_t, ind_x, Mtrain, N);
for (int i = 0;i < Npairs;i++)
{
pair = idx_pairs.row(i);
Predict_CGppe_Laplace(sigma, train_t, x, idx_global, ind_t, ind_x,
test_t, pair);
P(i) = p;
sum(pair(0)) += mustar(0);
count(pair(0)) += 1;
sum(pair(1)) += mustar(1);
count(pair(1)) += 1;
}
for (int i = 0;i < P.rows();i++)
{
if (P(i) > 0.5000001)
ypred(i) = 1;
else
ypred(i)=0;
}
fstar = sum.array() / count.array();
dsp(fstar,"fstar");
cout << endl << endl << "error = " << (GetDiff(ytrue, ypred)).sum() / ytrue.rows()<<endl;
// need for a plot function here ?
}
void generateBezierCurve()
{
ts = VectorXd::LinSpaced ( 21,0,1. );
t_x.resize ( ts.size() );
t_y.resize ( ts.size() );
t_z.resize ( ts.size() );
for ( int idx=0; idx< ts.size(); ++idx )
{
t_x ( idx ) = getValue ( ts ( idx ), pt_x );
t_y ( idx ) = getValue ( ts ( idx ), pt_y );
t_z ( idx ) = getValue ( ts ( idx ), pt_z );
}
}
VectorXd build_l_mode_a1etaa3(const VectorXd x_l, double H_l, double fc_l, double f_s, double eta, double a3, double asym, double gamma_l, const int l, VectorXd V){
/*
* This model includes:
* - Asymetry of Lorentzian asym
* - splitting a1
* - an Asphericity parameter eta
* - latitudinal effect a3
*/
const long Nxl=x_l.size();
VectorXd profile(Nxl), tmp(Nxl), tmp2(Nxl), result(Nxl), asymetry(Nxl);
double Qlm, clm;
result.setZero();
for(int m=-l; m<=l; m++){
if(l != 0){
Qlm=(l*(l+1) - 3*pow(m,2))/((2*l - 1)*(2*l + 3)); // accounting for eta
if(l == 1){
clm=m; // a3 for l=1
}
if(l == 2){
clm=(5*pow(m,3) - 17*m)/3.; // a3 for l=2
}
if(l == 3){
clm=0; // a3 NOT YET IMPLEMENTED FOR l=3
}
profile=(x_l - tmp.setConstant(fc_l*(1. + eta*Qlm) + m*f_s + clm*a3)).array().square();
profile=4*profile/pow(gamma_l,2);
} else{
profile=(x_l - tmp.setConstant(fc_l)).array().square();
profile=4*profile/pow(gamma_l,2);
}
if(asym == 0){ //Model with no asymetry
result=result+ H_l*V(m+l)* ((tmp.setConstant(1) + profile)).cwiseInverse();
} else{
tmp.setConstant(1);
asymetry=(tmp + asym*(x_l/fc_l - tmp)).array().square() + (tmp2.setConstant(0.5*gamma_l*asym/fc_l)).array().square();
result=result+ H_l*V(m+l)*asymetry.cwiseProduct(((tmp.setConstant(1) + profile)).cwiseInverse());
}
}
return result;
}
MultivariateFNormalSufficient::MultivariateFNormalSufficient(
const VectorXd& Fbar, double JF, const VectorXd& FM, int Nobs,
const MatrixXd& W, const MatrixXd& Sigma, double factor)
{
reset_flags();
N_=Nobs;
M_=Fbar.rows();
LOG( "MVN: sufficient statistics init with N=" << N_
<< " and M=" << M_ << std::endl);
CHECK( N_ > 0,
"please provide at least one observation per dimension");
CHECK( M_ > 0,
"please provide at least one variable");
set_factor(factor);
set_FM(FM);
set_Fbar(Fbar);
set_W(W);
set_jacobian(JF);
set_Sigma(Sigma);
}
static void writeMesh(const char *filename, const VectorXd &verts, const Matrix3Xi &faces)
{
ofstream ofs(filename);
for(int i=0; i<verts.size()/3; i++)
{
ofs << "v ";
for(int j=0; j<3; j++)
ofs << verts[3*i+j] << " ";
ofs << endl;
}
for(int i=0; i<faces.cols(); i++)
{
ofs << "f ";
for(int j=0; j<3; j++)
ofs << faces.coeff(j, i)+1 << " ";
ofs << endl;
}
}
MatrixXd MotionModel::dqomegadt_by_domega(VectorXd omega, double delta_t)
{
// This function calculates dq((w_k^C + omega^C) * delta_t)/domega_k^C. Javier book P130. A.17
// omega Modulus
double omega_norm = 0;
size_t omega_size = omega.size(), i;
MatrixXd dqomegadt_by_domegaRES(4,3);
for (i = 0; i < omega_size; ++i)
{
omega_norm += omega(i) * omega(i);
}
omega_norm = sqrt(omega_norm);
// Use generic ancillary functions to calculate components of Jacobian
dqomegadt_by_domegaRES << dq0_by_domegaA(omega(0), omega_norm, delta_t), dq0_by_domegaA(omega(1), omega_norm, delta_t), dq0_by_domegaA(omega(2), omega_norm, delta_t),
dqA_by_domegaA(omega(0), omega_norm, delta_t), dqA_by_domegaB(omega(0), omega(1), omega_norm, delta_t), dqA_by_domegaB(omega(0), omega(2), omega_norm, delta_t),
dqA_by_domegaB(omega(1), omega(0), omega_norm, delta_t), dqA_by_domegaA(omega(1), omega_norm, delta_t), dqA_by_domegaB(omega(1), omega(2), omega_norm, delta_t),
dqA_by_domegaB(omega(2), omega(0), omega_norm, delta_t), dqA_by_domegaB(omega(2), omega(1), omega_norm, delta_t), dqA_by_domegaA(omega(2), omega_norm, delta_t);
return dqomegadt_by_domegaRES;
}
double line_search(fcn_Rn_to_R f, VectorXd x0, VectorXd v0, VectorXd Dfx0 ) {
// Performs a line search. Takes in a function (f), initial point (x), and a
// direction (v). Tries to find a scalar value (a) such that f(x + av) is
// minimized.
//
double alpha = 1;
double beta = 0.2;
double tau = 0.5;
double fval = f(x0);
double stepsize = beta * Dfx0.dot(v0);
while ( f(x0 + alpha*v0) > fval + alpha*stepsize ) {
alpha = tau*alpha;
//cout << "Step size : " << alpha << endl;
}
return alpha;
}
pair<MatrixXd*,MatrixXd*> CutbySphere(MatrixXd &Atoms, double Radius, double x0,double y0,double z0 ){
//return ;
VectorXd Dx = Atoms.col(0).array() - x0;
VectorXd Dy = Atoms.col(1).array() - y0;
VectorXd Dz = Atoms.col(2).array() - z0;
//d2space=Dx.^2+Dy.^2+Dz.^2;
MatrixXd d2space = Dx.cwiseAbs2()+ Dy.cwiseAbs2() + Dz.cwiseAbs2();
MatrixXd D2space = d2space.array() - Radius*Radius;
MatrixXd d2plane = Dx.cwiseAbs2() + Dy.cwiseAbs2();
MatrixXd D2plane = d2plane.array() - Radius*Radius;
int range=Atoms.rows();
int j = 0, k = 0;
//need to improve here
MatrixXd* PickedAtoms = new MatrixXd(range,Atoms.cols());
MatrixXd* Unpicked = new MatrixXd(range,Atoms.cols());
for ( int i = 0 ; i < range; i ++){
//cout<<i<<" "<<Atoms(i,2)<<" "<<" "<<D2space(i,0)<<endl;
if (Atoms(i,2) > z0 && D2space(i,0) <= 0){
PickedAtoms->row(j) = Atoms.row(i);
j++;
}else if (Atoms(i,2) <= z0 && D2plane(i,0) <= 0){
PickedAtoms->row(j) = Atoms.row(i);
j++;
}else{
Unpicked->row(k) = Atoms.row(i);
k++;
}
}
PickedAtoms->conservativeResize(j,Atoms.cols());
Unpicked->conservativeResize(k,Atoms.cols());
return pair<MatrixXd*,MatrixXd*> (PickedAtoms,Unpicked);
}
void addJointSoftLimits(const JointSoftLimitParams ¶ms, const DrakeRobotState &robot_state, const VectorXd &q_des, std::vector<SupportStateElement,Eigen::aligned_allocator<SupportStateElement>> &supports, std::vector<drake::lcmt_joint_pd_override> &joint_pd_override) {
Matrix<bool, Dynamic, 1> has_joint_override = Matrix<bool, Dynamic, 1>::Zero(q_des.size());
for (std::vector<drake::lcmt_joint_pd_override>::iterator it = joint_pd_override.begin(); it != joint_pd_override.end(); ++it) {
has_joint_override(it->position_ind - 1) = true;
}
for (int i=0; i < params.lb.size(); i++) {
if (!has_joint_override(i) && params.enabled(i)) {
int disable_body_1idx = params.disable_when_body_in_support(i);
if (disable_body_1idx == 0 || !inSupport(supports, disable_body_1idx - 1)) {
double w_lb = 0;
double w_ub = 0;
if (!std::isinf(params.lb(i))) {
w_lb = logisticSigmoid(params.weight(i), params.k_logistic(i), params.lb(i), robot_state.q(i));
}
if (!std::isinf(params.ub(i))) {
w_ub = logisticSigmoid(params.weight(i), params.k_logistic(i), robot_state.q(i), params.ub(i));
}
double weight = std::max(w_ub, w_lb);
drake::lcmt_joint_pd_override override;
override.position_ind = i + 1;
override.qi_des = q_des(i);
int contactPhi(RigidBodyManipulator* r, SupportStateElement& supp, void *map_ptr, VectorXd &phi, double terrain_height)
{
int nc = static_cast<int>(supp.contact_pts.size());
phi.resize(nc);
if (nc<1) return nc;
Vector3d contact_pos,pos,normal;
int i=0;
for (std::vector<Vector4d,aligned_allocator<Vector4d>>::iterator pt_iter=supp.contact_pts.begin(); pt_iter!=supp.contact_pts.end(); pt_iter++) {
r->forwardKin(supp.body_idx,*pt_iter,0,contact_pos);
collisionDetect(map_ptr,contact_pos,pos,&normal,terrain_height);
pos -= contact_pos; // now -rel_pos in matlab version
phi(i) = pos.norm();
if (pos.dot(normal)>0)
phi(i)=-phi(i);
i++;
}
return nc;
}
void Dmp::statesAsTrajectory(const VectorXd& ts, const MatrixXd& x_in, const MatrixXd& xd_in, Trajectory& trajectory) const {
int n_time_steps = ts.rows();
int n_dims = x_in.cols();
assert(n_time_steps==x_in.rows());
assert(n_time_steps==xd_in.rows());
assert(n_dims==xd_in.cols());
// Left column is time
Trajectory new_trajectory(
ts,
// y_out (see function above)
x_in.SPRINGM_Y(n_time_steps),
// yd_out (see function above)
xd_in.SPRINGM_Y(n_time_steps),
// ydd_out (see function above)
xd_in.SPRINGM_Z(n_time_steps)/tau()
);
trajectory = new_trajectory;
}
void cRBLayer::plotVector(VectorXd& x, const char *name){
FILE* fout=fopen("tmpdata.dat","w");
int size = sqrt(x.size());
int k=0;
for(int i=0;i<size;i++){
for(int j=0;j<size;j++,k++){
fprintf(fout,"%f ",x(k));
}
fprintf(fout,"\n");
}
fclose(fout);
string plotscript("set terminal png size 400,250\nset output ");
plotscript+=string("'")+string(name)+string("'")+string("\n");
plotscript+=string("set palette gray\nunset colorbox\nset cbrange[0:1.0]\n");
plotscript+=string("plot 'tmpdata.dat' matrix with image");
ofstream sfout("vectorplotscript.gnu",ios::out);
sfout<<plotscript;
sfout.close();
system("gnuplot vectorplotscript.gnu");
system("rm tmpdata.dat");
}
void ImplicitEuler::ImplicitTVtoX(VectorXd& x_tv, MatrixXd& TVk){
x_tv.setZero();
for(unsigned int i = 0; i < M.tets.size(); i++){
Vector4i indices = M.tets[i].verticesIndex;
x_tv(3*indices(0)) = TVk.row(indices(0))[0];
x_tv(3*indices(0)+1) = TVk.row(indices(0))[1];
x_tv(3*indices(0)+2) = TVk.row(indices(0))[2];
x_tv(3*indices(1)) = TVk.row(indices(1))[0];
x_tv(3*indices(1)+1) = TVk.row(indices(1))[1];
x_tv(3*indices(1)+2) = TVk.row(indices(1))[2];
x_tv(3*indices(2)) = TVk.row(indices(2))[0];
x_tv(3*indices(2)+1) = TVk.row(indices(2))[1];
x_tv(3*indices(2)+2) = TVk.row(indices(2))[2];
x_tv(3*indices(3)) = TVk.row(indices(3))[0];
x_tv(3*indices(3)+1) = TVk.row(indices(3))[1];
x_tv(3*indices(3)+2) = TVk.row(indices(3))[2];
}
}
double HmcSampler::_verifyConstraints(const VectorXd & b){
double r =0;
for (int i=0; i != quadraticConstraints.size(); i++ ){
QuadraticConstraint qc = quadraticConstraints[i];
double check = ((b.transpose())*(qc.A))*b + (qc.B).dot(b) + qc.C;
if (i==0 || check < r) {
r = check;
}
}
for (int i=0; i != linearConstraints.size(); i++ ){
LinearConstraint lc = linearConstraints[i];
double check = (lc.f).dot(b) + lc.g;
if (i==0 || check < r) {
r = check;
}
}
return r;
}
/* ******************************************************************************************** */
void printResult (State& s, VectorXd& X, VectorXd& Y) {
ofstream res ("res");
res << obj0 << "\n0" << endl;
for(int i = 0; i < X.rows(); i++) {
if(s.order[i] < 0) {
int stick_id = (-s.order[i]) / 10;
int gear_id = (-s.order[i]) % 10;
res << -abs(X(i)) << "\n" << Y(i) << "\n" << radii[gear_id] << "\n";
if(X(i) < 0)
res << -lengths[stick_id] << endl;
else
res << lengths[stick_id] << endl;
}
else
res << X(i) << "\n" << Y(i) << "\n" << radii[s.order[i]] << "\n0" << endl;
}
res << objn << "\n0" << endl;
for(int i = 0; i < obs.size(); i++)
res << obs[i](0) << "\n" << obs[i](1) << "\n" << -obs[i](2) << "\n0" << endl;
res.close();
}
MultivariateFNormalSufficient::MultivariateFNormalSufficient(
const VectorXd& Fbar, double JF, const VectorXd& FM, int Nobs,
const MatrixXd& W, const MatrixXd& Sigma, double factor)
: base::Object("Multivariate Normal distribution %1%")
{
reset_flags();
N_=Nobs;
M_=Fbar.rows();
IMP_LOG_TERSE( "MVN: sufficient statistics init with N=" << N_
<< " and M=" << M_ << std::endl);
IMP_USAGE_CHECK( N_ > 0,
"please provide at least one observation per dimension");
IMP_USAGE_CHECK( M_ > 0,
"please provide at least one variable");
set_factor(factor);
set_FM(FM);
set_Fbar(Fbar);
set_W(W);
set_jacobian(JF);
set_Sigma(Sigma);
use_cg_=false;
}
double CostFromErrFunc::value(const vector<double>& xin) {
VectorXd x = getVec(xin, vars_);
VectorXd err = f_->call(x);
if (coeffs_.size()>0) err.array() *= coeffs_.array();
switch (pen_type_) {
case SQUARED:
return err.array().square().sum();
case ABS:
return err.array().abs().sum();
case HINGE:
return err.cwiseMax(VectorXd::Zero(err.size())).sum();
default:
assert(0 && "unreachable");
}
return 0; // avoid compiler warning
}
Trajectory getDemoTrajectory(const VectorXd& ts)
{
bool use_viapoint_traj= false;
Trajectory trajectory;
int n_dims=0;
if (use_viapoint_traj)
{
n_dims = 1;
VectorXd y_first = VectorXd::Zero(n_dims);
VectorXd y_last = VectorXd::Ones(n_dims);
double viapoint_time = 0.25;
double viapoint_location = 0.5;
VectorXd y_yd_ydd_viapoint = VectorXd::Zero(3*n_dims);
y_yd_ydd_viapoint.segment(0*n_dims,n_dims).fill(viapoint_location); // y
trajectory = Trajectory::generatePolynomialTrajectoryThroughViapoint(ts,y_first,y_yd_ydd_viapoint,viapoint_time,y_last);
}
else
{
n_dims = 2;
VectorXd y_first = VectorXd::LinSpaced(n_dims,0.0,0.7); // Initial state
VectorXd y_last = VectorXd::LinSpaced(n_dims,0.4,0.5); // Final state
trajectory = Trajectory::generateMinJerkTrajectory(ts, y_first, y_last);
}
// Generated trajectory, now generate extra dimensions in width
MatrixXd misc(ts.rows(),n_dims);
for (int ii=0; ii<misc.rows(); ii++)
{
misc(ii,0) = (1.0*ii)/misc.rows();
if (n_dims>1)
misc(ii,1) = sin((8.0*ii)/misc.rows());
}
trajectory.set_misc(misc);
return trajectory;
}
void CstepPrep(
const MatrixXd& x,
const VectorXd& y,
const VectorXi& dIn,
VectorXd& dP,
const int& h0
){
const int n=x.rows(),p=x.cols();
double w1,w0;
int w2=1,i,j=0;
MatrixXd xSub(h0,p);
VectorXd ySub(h0);
for(i=0;i<h0;i++) xSub.row(i)=x.row(dIn(i));
for(i=0;i<h0;i++) ySub(i)=y(dIn(i));
MatrixXd Sig(p,p);
//Sig=xSub.adjoint()*xSub;
//LDLT<MatrixXd> chol=Sig.ldlt();
Sig.setZero().selfadjointView<Lower>().rankUpdate(xSub.transpose());
LLT<MatrixXd> chol=Sig.llt();
VectorXd m_cf(p);
m_cf=chol.solve(xSub.adjoint()*ySub);
dP=((x*m_cf).array()-y.array()).abs();
}
void RotationMatrix::compute(const VectorXd &u, SparseMatrix<double>& R)const{
// compute rs coordinates of each element
VectorXd RS_u;
_warper->computeRSCoord(u, RS_u, false);
// compute the rotational vector of each node.
const int node_num = u.size()/3;
vector<Vector3d> w(node_num);
vector<int> neighbor_ele_of_node(node_num);
for (int i = 0; i < node_num; ++i){
w[i].setZero();
neighbor_ele_of_node[i] = 0;
}
for (int ei = 0; ei < _tet_mesh->tets().size(); ++ei){
const Vector4i ele = _tet_mesh->tets()[ei];
for (int j = 0; j < 4; ++j){
const int vi = ele[j];
w[vi][0] += RS_u[ei*9+0];
w[vi][1] += RS_u[ei*9+1];
w[vi][2] += RS_u[ei*9+2];
neighbor_ele_of_node[vi] ++;
}
}
for (int i = 0; i < node_num; ++i){
w[i] = w[i]/(double)(neighbor_ele_of_node[i]);
}
// compute the rotational matrix of each node
mat3r Ri;
vector<mat3r> R_vec;
R_vec.reserve(node_num);
for (int i = 0; i < node_num; ++i){
R_vec.push_back(ComputeBj::compute_exp(w[i],Ri));
}
EIGEN3EXT::eye(R,R_vec);
}
GTEST_TEST(TestConvexHull, testRandomConvexCombinations) {
// Generate a set of points, then find a random convex combination of those
// points, and verify that it's correctly reported as being inside the
// convex hull
for (int i = 2; i < 50; ++i) {
for (int j = 0; j < 500; ++j) {
MatrixXd pts = MatrixXd::Random(2, i);
VectorXd weights = VectorXd::Random(i);
if (weights.minCoeff() < 0) {
weights = weights.array() -
weights.minCoeff(); // make sure they're all nonnegative
}
weights = weights.array() / weights.sum();
Vector2d q = pts * weights;
EXPECT_TRUE(inConvexHull(pts, q, 1e-8));
}
}
}
void World::setStateForJacobian(VectorXd& world_state) {
int ind = 0;
for (int i = 0; i < threads.size(); i++) {
VectorXd state;
state = world_state.segment(ind, world_state(ind));
threads[i]->setState(state);
ind += state.size();
}
for (int i = 0; i < cursors.size(); i++) {
if(cursors[i]->isAttached()) {
VectorXd state;
state = world_state.segment(ind, world_state(ind));
cursors[i]->setState(state);
cursors[i]->end_eff->setState(state);
ind += state.size();
}
}
assert(ind == world_state.size());
}
// Plots the mesh with an N-RoSy field and its singularities on top
// The constrained faces (b) are colored in red.
void plot_mesh_nrosy(igl::Viewer& viewer, MatrixXd& V, MatrixXi& F, int N, MatrixXd& PD1, VectorXd& S, VectorXi& b)
{
// Clear the mesh
viewer.data.clear();
viewer.data.set_mesh(V,F);
// Expand the representative vectors in the full vector set and plot them as lines
double avg = igl::avg_edge_length(V, F);
MatrixXd Y;
representative_to_nrosy(V, F, PD1, N, Y);
MatrixXd B;
igl::barycenter(V,F,B);
MatrixXd Be(B.rows()*N,3);
for(unsigned i=0; i<B.rows();++i)
for(unsigned j=0; j<N; ++j)
Be.row(i*N+j) = B.row(i);
viewer.data.add_edges(Be,Be+Y*(avg/2),RowVector3d(0,0,1));
// Plot the singularities as colored dots (red for negative, blue for positive)
for (unsigned i=0; i<S.size();++i)
{
if (S(i) < -0.001)
viewer.data.add_points(V.row(i),RowVector3d(1,0,0));
else if (S(i) > 0.001)
viewer.data.add_points(V.row(i),RowVector3d(0,1,0));
}
// Highlight in red the constrained faces
MatrixXd C = MatrixXd::Constant(F.rows(),3,1);
for (unsigned i=0; i<b.size();++i)
C.row(b(i)) << 1, 0, 0;
viewer.data.set_colors(C);
}
/** \todo Document FunctionApproximatorGMR::normalPDFDamped
*/
double FunctionApproximatorGMR::normalPDFDamped(const VectorXd& mu, const MatrixXd& covar, const VectorXd& input)
{
if(covar.determinant() > 0) // It is invertible
{
MatrixXd covar_inverse = covar.inverse();
double output = exp(-0.5*(input-mu).transpose()*covar_inverse*(input-mu));
// Check that:
// if output == 0.0
// return 0.0;
// For invertible matrices (which covar apparently was), det(A^-1) = 1/det(A)
// Hence the 1.0/covar_inverse.determinant() below
// ( (2\pi)^N*|\Sigma| )^(-1/2)
output *= pow(pow(2*M_PI,mu.size())/(covar_inverse.determinant()),-0.5);
return output;
}
else
{
//cerr << "WARNING: FunctionApproximatorGMR::normalPDFDamped output close to singularity..." << endl;
return std::numeric_limits<double>::min();
}
}
void OMNI::subsequenceQuery(VectorXd qs, double epsilon)
{
double low, up;
MatrixXd qsMatrix;
qsMatrix.resize(1,qs.size());
qsMatrix.row(0) = qs;
qsdf.resize(data.fociNum);
for(int i = 0; i < data.fociNum; ++i)
{
qsdf = data.fociTS.rowwise().operator-(qsMatrix.row(0)).array().square().matrix().rowwise().sum();
}
tsCount.resize(data.tsNum, data.totalLen - data.tsLen + 1);
tsCount.setZero();
for(int a = 0; a < data.fociNum; ++a)
{
low = qsdf[a] - epsilon;
up = qsdf[a] + epsilon;
forest[a].rangeQuery(low, up, &tsCount);
}
queryResult.resize(0);
for(int x = 0; x < data.tsNum; ++x)
{
for(int y = 0; y < data.totalLen - data.tsLen + 1; ++y)
{
if(tsCount(x,y) == data.fociNum)
{
queryResult.push_back(x*1000+y);
}
}
}
}