Trajectory Trajectory::generatePolynomialTrajectory(const VectorXd& ts, const VectorXd& y_from, const VectorXd& yd_from, const VectorXd& ydd_from,
const VectorXd& y_to, const VectorXd& yd_to, const VectorXd& ydd_to)
{
VectorXd a0 = y_from;
VectorXd a1 = yd_from;
VectorXd a2 = ydd_from / 2;
VectorXd a3 = -10 * y_from - 6 * yd_from - 2.5 * ydd_from + 10 * y_to - 4 * yd_to + 0.5 * ydd_to;
VectorXd a4 = 15 * y_from + 8 * yd_from + 2 * ydd_from - 15 * y_to + 7 * yd_to - ydd_to;
VectorXd a5 = -6 * y_from - 3 * yd_from - 0.5 * ydd_from + 6 * y_to - 3 * yd_to + 0.5 * ydd_to;
int n_time_steps = ts.size();
int n_dims = y_from.size();
MatrixXd ys(n_time_steps,n_dims), yds(n_time_steps,n_dims), ydds(n_time_steps,n_dims);
for (int i = 0; i < ts.size(); i++)
{
double t = (ts[i] - ts[0]) / (ts[n_time_steps - 1] - ts[0]);
ys.row(i) = a0 + a1 * t + a2 * pow(t, 2) + a3 * pow(t, 3) + a4 * pow(t, 4) + a5 * pow(t, 5);
yds.row(i) = a1 + 2 * a2 * t + 3 * a3 * pow(t, 2) + 4 * a4 * pow(t, 3) + 5 * a5 * pow(t, 4);
ydds.row(i) = 2 * a2 + 6 * a3 * t + 12 * a4 * pow(t, 2) + 20 * a5 * pow(t, 3);
}
yds /= (ts[n_time_steps - 1] - ts[0]);
ydds /= pow(ts[n_time_steps - 1] - ts[0], 2);
return Trajectory(ts, ys, yds, ydds);
}
Trajectory Trajectory::generatePolynomialTrajectoryThroughViapoint(const VectorXd& ts, const VectorXd& y_from, const VectorXd& y_yd_ydd_viapoint, double viapoint_time, const VectorXd& y_to)
{
int n_dims = y_from.size();
assert(n_dims==y_to.size());
assert(3*n_dims==y_yd_ydd_viapoint.size()); // Contains y, yd and ydd, so *3
int n_time_steps = ts.size();
int viapoint_time_step = 0;
while (viapoint_time_step<n_time_steps && ts[viapoint_time_step]<viapoint_time)
viapoint_time_step++;
if (viapoint_time_step>=n_time_steps)
{
cerr << __FILE__ << ":" << __LINE__ << ":";
cerr << "ERROR: the time vector does not contain any time smaller than " << viapoint_time << ". Returning min-jerk trajectory WITHOUT viapoint." << endl;
return Trajectory();
}
VectorXd yd_from = VectorXd::Zero(n_dims);
VectorXd ydd_from = VectorXd::Zero(n_dims);
VectorXd y_viapoint = y_yd_ydd_viapoint.segment(0*n_dims,n_dims);
VectorXd yd_viapoint = y_yd_ydd_viapoint.segment(1*n_dims,n_dims);
VectorXd ydd_viapoint = y_yd_ydd_viapoint.segment(2*n_dims,n_dims);
VectorXd yd_to = VectorXd::Zero(n_dims);
VectorXd ydd_to = VectorXd::Zero(n_dims);
Trajectory traj = Trajectory::generatePolynomialTrajectory(ts.segment(0, viapoint_time_step + 1), y_from, yd_from, ydd_from, y_viapoint, yd_viapoint, ydd_viapoint);
traj.append(Trajectory::generatePolynomialTrajectory(ts.segment(viapoint_time_step, n_time_steps - viapoint_time_step), y_viapoint, yd_viapoint, ydd_viapoint, y_to, yd_to, ydd_to));
return traj;
}
Trajectory Trajectory::generateMinJerkTrajectory(const VectorXd& ts, const VectorXd& y_from, const VectorXd& y_to)
{
int n_time_steps = ts.size();
int n_dims = y_from.size();
MatrixXd ys(n_time_steps,n_dims), yds(n_time_steps,n_dims), ydds(n_time_steps,n_dims);
double D = ts[n_time_steps-1];
ArrayXd tss = (ts/D).array();
ArrayXd A = y_to.array()-y_from.array();
for (int i_dim=0; i_dim<n_dims; i_dim++)
{
// http://noisyaccumulation.blogspot.fr/2012/02/how-to-decompose-2d-trajectory-data.html
ys.col(i_dim) = y_from[i_dim] + A[i_dim]*( 6*tss.pow(5) -15*tss.pow(4) +10*tss.pow(3));
yds.col(i_dim) = (A[i_dim]/D)*( 30*tss.pow(4) -60*tss.pow(3) +30*tss.pow(2));
ydds.col(i_dim) = (A[i_dim]/(D*D))*(120*tss.pow(3) -180*tss.pow(2) +60*tss );
}
return Trajectory(ts,ys,yds,ydds);
}
double Mesh::triangleInequalityLineSearch(const VectorXd &g, const VectorXd &dg) const
{
assert(g.size() == numedges());
assert(dg.size() == numedges());
double maxt = std::numeric_limits<double>::infinity();
for(OMMesh::FaceIter fi = mesh_->faces_begin(); fi != mesh_->faces_end(); ++fi)
{
vector<double> gs;
vector<double> dgs;
for(OMMesh::FaceEdgeIter fei = mesh_->fe_iter(fi.handle()); fei; ++fei)
{
gs.push_back(g[fei.handle().idx()]);
dgs.push_back(dg[fei.handle().idx()]);
}
assert(gs.size() == 3);
for(int i=0; i<3; i++)
{
int idx[3];
for(int j=0; j<3; j++)
{
idx[j] = (i+j)%3;
}
double thismax = triangleInequalityLineSearch(gs[idx[0]], gs[idx[1]], gs[idx[2]],
dgs[idx[0]], dgs[idx[1]], dgs[idx[2]]);
maxt = std::min(maxt, thismax);
}
}
return maxt;
}
VectorXd HarmonicGradient(const vector<spring> &springlist,
const VectorXd &XY,
const double g11,
const double g12,
const double g22)
{
VectorXd gradE(XY.size());
for(int i=0;i<gradE.size();i++){
gradE(i)=0;
}
VectorXd X(XY.size()/2);
VectorXd Y(XY.size()/2);
X=XY.head(XY.size()/2);
Y=XY.tail(XY.size()/2);
for(int i=0;i<springlist.size();i++){
int one=springlist[i].one;
int two=springlist[i].two;
int num=XY.size()/2;
double dx=X(one)-(X(two)+springlist[i].wlr);
double dy=Y(one)-(Y(two)+springlist[i].wud);
double k=springlist[i].k;
double L=springlist[i].rlen;
double dist=sqrt( g11*dx*dx+ 2*g12*dx*dy+ g22*dy*dy );
double gradx= k*(dist-L)*(g11*dx+g12*dy)/dist;
double grady= k*(dist-L)*(g22*dy+g12*dx)/dist;
gradE(one) += gradx;
gradE(two) -= gradx;
gradE(one+num) += grady;
gradE(two+num) -= grady;
}
return gradE;
}
int operator()(const VectorXd &x, VectorXd &fvec) const
{
assert( x.size() == _dim );
assert( (unsigned int) fvec.size() == 6);
Eigen::Matrix<double,12,1> solution = _a;
for(size_t i = 0; i < _dim; i++)
solution += x[i]*_V.col(12-_dim+i);
points_t ccam;
for(size_t i = 0; i<4; i++)
ccam.push_back(solution.block<3,1>(i*3,0));
Eigen::Vector3d diffw;
Eigen::Vector3d diffc;
size_t index = 0;
for(size_t i = 0; i<3; i++)
{
for(size_t j = i+1; j < 4; j++)
{
diffw = _c[i]-_c[j];
diffc = ccam[i]-ccam[j];
fvec[index++] = diffw.dot(diffw)-diffc.dot(diffc);
}
}
return 0;
}
void mexFunction(int nlhs, mxArray* plhs[], int nrhs, const mxArray *prhs[])
{
if(nrhs!=3 || nlhs != 8)
{
mexErrMsgIdAndTxt("Drake:testMultipleTimeLinearPostureConstrainttmex:BadInputs","Usage [num_cnst,cnst_val,iAfun,jAvar,A,cnst_name,lb,ub] = testMultipleTimeLinearPostureConstraintmex(kinCnst,q,t)");
}
MultipleTimeLinearPostureConstraint* cnst = (MultipleTimeLinearPostureConstraint*) getDrakeMexPointer(prhs[0]);
int n_breaks = mxGetNumberOfElements(prhs[2]);
double* t_ptr = new double[n_breaks];
memcpy(t_ptr,mxGetPrSafe(prhs[2]),sizeof(double)*n_breaks);
int nq = cnst->getRobotPointer()->num_positions;
MatrixXd q(nq,n_breaks);
if(mxGetM(prhs[1]) != nq || mxGetN(prhs[1]) != n_breaks)
{
mexErrMsgIdAndTxt("Drake:testMultipleTimeLinearPostureConstraintmex:BadInputs","Argument 2 must be of size nq*n_breaks");
}
memcpy(q.data(),mxGetPrSafe(prhs[1]),sizeof(double)*nq*n_breaks);
int num_cnst = cnst->getNumConstraint(t_ptr,n_breaks);
VectorXd c(num_cnst);
cnst->feval(t_ptr,n_breaks,q,c);
VectorXi iAfun;
VectorXi jAvar;
VectorXd A;
cnst->geval(t_ptr,n_breaks,iAfun,jAvar,A);
std::vector<std::string> cnst_names;
cnst->name(t_ptr,n_breaks,cnst_names);
VectorXd lb(num_cnst);
VectorXd ub(num_cnst);
cnst->bounds(t_ptr,n_breaks,lb,ub);
VectorXd iAfun_tmp(iAfun.size());
VectorXd jAvar_tmp(jAvar.size());
for(int i = 0;i<iAfun.size();i++)
{
iAfun_tmp(i) = (double) iAfun(i)+1;
jAvar_tmp(i) = (double) jAvar(i)+1;
}
plhs[0] = mxCreateDoubleScalar((double) num_cnst);
plhs[1] = mxCreateDoubleMatrix(num_cnst,1,mxREAL);
memcpy(mxGetPrSafe(plhs[1]),c.data(),sizeof(double)*num_cnst);
plhs[2] = mxCreateDoubleMatrix(iAfun_tmp.size(),1,mxREAL);
memcpy(mxGetPrSafe(plhs[2]),iAfun_tmp.data(),sizeof(double)*iAfun_tmp.size());
plhs[3] = mxCreateDoubleMatrix(jAvar_tmp.size(),1,mxREAL);
memcpy(mxGetPrSafe(plhs[3]),jAvar_tmp.data(),sizeof(double)*jAvar_tmp.size());
plhs[4] = mxCreateDoubleMatrix(A.size(),1,mxREAL);
memcpy(mxGetPrSafe(plhs[4]),A.data(),sizeof(double)*A.size());
int name_ndim = 1;
mwSize name_dims[] = {(mwSize) num_cnst};
plhs[5] = mxCreateCellArray(name_ndim,name_dims);
mxArray *name_ptr;
for(int i = 0;i<num_cnst;i++)
{
name_ptr = mxCreateString(cnst_names[i].c_str());
mxSetCell(plhs[5],i,name_ptr);
}
plhs[6] = mxCreateDoubleMatrix(num_cnst,1,mxREAL);
plhs[7] = mxCreateDoubleMatrix(num_cnst,1,mxREAL);
memcpy(mxGetPrSafe(plhs[6]),lb.data(),sizeof(double)*num_cnst);
memcpy(mxGetPrSafe(plhs[7]),ub.data(),sizeof(double)*num_cnst);
delete[] t_ptr;
}
void f_u(VectorXd &val, double * x_array, double * u_array, VectorXd const &y, double K, double S0, int n){
gsl_interp_accel *my_accel_ptr = gsl_interp_accel_alloc ();
gsl_spline *my_spline_ptr = gsl_spline_alloc (gsl_interp_cspline, n);
gsl_spline_init (my_spline_ptr, x_array, u_array, n);
val=VectorXd(y.size());
int j=0;
int k=0;
while (y(j)<x_array[0]) {
val(k)=payoff(y(k),K,S0);
++k;
++j;
}
while (j<y.size() && y(j)<x_array[n-1]) {
val(k)=gsl_spline_eval(my_spline_ptr, y(k) , my_accel_ptr);
++j;
++k;
}
for (int i=k; i<y.size(); ++i) {
val(i)=0;
}
gsl_spline_free(my_spline_ptr);
gsl_interp_accel_free(my_accel_ptr);
return;
}
VectorXd math_ext::interpolate_y(const VectorXd &x_in, const VectorXd &y_in, const VectorXd &y_out)
{
const int n=x_in.size(), m=y_out.size();
VectorXd x_out(m);
for(int n1=0;n1<m;++n1)
{
if(y_out(n1)<=y_in(0))
{
//x_out(n1)=x_in(0)+(y_out(n1)-y_in(0))*(x_in(1)-x_in(0))/(y_in(1)-y_in(0));
x_out(n1)=x_in(0);
continue;
}
else if(y_out(n1)>=y_in(n-1))
{
//x_out(n1)=x_in(n-1)+(y_out(n1)-y_in(n-1))*(x_in(n-2)-x_in(n-1))/(y_in(n-2)-y_in(n-1));
x_out(n1)=x_in(n-1);
continue;
}
for(int n2=1;n2<n;++n2)
{
if(y_out(n1)>y_in(n2-1) && y_out(n1)<=y_in(n2))
{
x_out(n1)=x_in(n2-1)+(x_in(n2)-x_in(n2-1))*(y_out(n1)-y_in(n2-1))/(y_in(n2)-y_in(n2-1));
break;
}
else
continue;
}
}
return(x_out);
}
int operator()(const VectorXd &b, VectorXd &fvec) const {
ASSERT(b.size() == unknowns);
ASSERT(fvec.size() == datasetLen);
for(int i = 0; i < 35; i++)
fvec[i] = b[0]/b[1] * exp(-0.5*(x[i]-b[2])*(x[i]-b[2])/(b[1]*b[1])) - y[i];
return 0;
}
/**
* reconstruct the displacements u in Euler space with RS coordinates y
* provided.
*
* @param y the RS coordinates.
* @param u the constructed Euler coordinates represents the displacements.
*
* @return true if construction is success.
*/
bool RS2Euler::reconstruct(const VectorXd &y, VectorXd &u){
assert (tetmesh != NULL);
const int node_numx3 = tetmesh->nodes().size()*3;
bool succ = true;
// assemble the right_side
VectorXd b;
assemble_b(y,b);
assert_eq(VG_t.cols(),b.size());
assert_eq(VG_t.rows(),node_numx3);
VectorXd right_side(node_numx3 + numFixedDofs());
right_side.segment(0, node_numx3) = VG_t*b;
right_side.segment(node_numx3,numFixedDofs()).setZero();
right_side.segment(node_numx3,barycenter_uc.size()) = barycenter_uc;
// solve A*u = right_side
assert_eq (right_side.size(), A_solver.rows());
u = A_solver.solve(right_side);
// get the result
if(A_solver.info()!=Eigen::Success) {
succ = false;
u.resize(node_numx3);
u.setZero();
ERROR_LOG("failed to solve for A X = P.");
}else{
assert_gt(u.size(), node_numx3);
const VectorXd x = u.head(node_numx3);
u = x;
}
return succ;
}
//The unvectorized version
void fresnelApprox(const VectorXd &t, VectorXd *s, VectorXd *c)
{
s->resize(t.size());
c->resize(t.size());
for(int i = 0; i < t.size(); ++i)
fresnelApprox(t[i], &((*s)[i]), &((*c)[i]));
}
void pca_small(MatrixXd &B, int method, MatrixXd& U, VectorXd &d, bool verbose)
{
if(method == METHOD_SVD)
{
verbose && std::cout << timestamp() << " SVD begin" << std::endl;
JacobiSVD<MatrixXd> svd(B, ComputeThinU | ComputeThinV);
U = svd.matrixU();
MatrixXd V = svd.matrixV();
d = svd.singularValues().array().pow(2);
verbose && std::cout << timestamp() << " SVD done" << std::endl;
}
else if(method == METHOD_EIGEN)
{
verbose && std::cout << timestamp() << " Eigen-decomposition begin" << std::endl;
MatrixXd BBT = B * B.transpose();
verbose && std::cout << timestamp() << " dim(BBT): " << dim(BBT) << std::endl;
SelfAdjointEigenSolver<MatrixXd> eig(BBT);
// The eigenvalues come out sorted in *increasing* order,
// but we need decreasing order
VectorXd eval = eig.eigenvalues();
MatrixXd evec = eig.eigenvectors();
d.resize(eval.size());
U.resize(BBT.rows(), BBT.rows());
unsigned int k = 0, s = d.size();
for(unsigned int i = d.size() - 1 ; i != -1 ; --i)
{
// we get eigenvalues, which are the squared singular values
d(k) = eval(i);
U.col(k) = evec.col(i);
k++;
}
}
}
double EEMS::test_prior(const MatrixXd &mSeeds, const VectorXd &mEffcts, const double mrateMu,
const MatrixXd &qSeeds, const VectorXd &qEffcts,
const double df, const double sigma2, const double mrateS2, const double qrateS2) const {
bool inrange = true;
int qtiles = qEffcts.size();
int mtiles = mEffcts.size();
// First check that all parameters fall into their support range
for ( int i = 0 ; i < qtiles ; i++ ) {
if (!habitat.in_point(qSeeds(i,0),qSeeds(i,1))) { inrange = false; }
}
for ( int i = 0 ; i < mtiles ; i++ ) {
if (!habitat.in_point(mSeeds(i,0),mSeeds(i,1))) { inrange = false; }
}
if (qEffcts.cwiseAbs().minCoeff()>params.qEffctHalfInterval) { inrange = false; }
if (mEffcts.cwiseAbs().minCoeff()>params.mEffctHalfInterval) { inrange = false; }
if (abs(mrateMu)>params.mrateMuHalfInterval) { inrange = false; }
if ((df<params.dfmin) || (df>params.dfmax)) { inrange = false; }
if (!inrange) { return (-Inf); }
// Then compute the prior, on the log scale
double logpi = - log(df)
+ dnegbinln(mtiles,params.negBiSize,params.negBiProb)
+ dnegbinln(qtiles,params.negBiSize,params.negBiProb)
+ dinvgamln(mrateS2,params.mrateShape_2,params.mrateScale_2)
+ dinvgamln(qrateS2,params.qrateShape_2,params.qrateScale_2)
+ dmvnormln(mEffcts,VectorXd::Zero(mtiles),mrateS2*MatrixXd::Identity(mtiles,mtiles))
+ dmvnormln(qEffcts,VectorXd::Zero(qtiles),qrateS2*MatrixXd::Identity(qtiles,qtiles))
+ dinvgamln(sigma2,params.sigmaShape_2,params.sigmaScale_2);
return (logpi);
}
Eigen::VectorXd normProbVector(VectorXd P_vec)
{
VectorXd P_norm = P_vec;
if (P_vec == Eigen::VectorXd::Zero(P_vec.size())){
}
else{
double P_positive = 0; double P_negative = 0;
for (int row_idx = 0; row_idx < P_vec.size(); row_idx++){
P_positive = (P_vec(row_idx) > 0) ? (P_positive + P_vec(row_idx)) : P_positive;
P_negative = (P_vec(row_idx) > 0) ? P_negative : (P_negative + P_vec(row_idx));
}
if (fabs(P_positive) < fabs(P_negative)){
P_norm = -P_vec / fabs(P_negative);
}
else{
P_norm = P_vec / fabs(P_positive);
}
for (int row_idx = 0; row_idx < P_vec.size(); row_idx++){
P_norm(row_idx) = (P_norm(row_idx)<0) ? 0 : P_norm(row_idx);
}
}
return P_norm;
}
void nilss_solve(const std::vector<MatrixXd>& R,
const std::vector<MatrixXd>& D,
const std::vector<VectorXd>& b,
const std::vector<VectorXd>& c,
std::vector<VectorXd>& a)
{
int n = R.size();
assert(D.size() == n + 1);
assert(b.size() == n);
assert(c.size() == n + 1);
std::unique_ptr<MatrixXd> kkt = assemble_kkt(R, D);
std::unique_ptr<VectorXd> rhs = assemble_rhs(b, c);
typedef SparseMatrix<double> SpMat;
SpMat A(kkt->sparseView());
SparseLU<SparseMatrix<double>> solver;
solver.analyzePattern(A);
solver.factorize(A);
VectorXd sol = solver.solve(*rhs);
//VectorXd sol = kkt->partialPivLu().solve(*rhs);
assert(sol.size() % (2 * n + 1) == 0);
int m = sol.size() / (2 * n + 1);
a.empty();
a.reserve(n + 1);
for (int i = 0; i <= n; ++ i) {
a.push_back(sol.segment(i * m, m));
}
}
bool drwnTableFactor::load(drwnXMLNode& xml)
{
// free existing storage
if ((_storage == NULL) && (_data != NULL)) {
delete[] _data;
_data = NULL;
#ifdef DRWN_FACTOR_DEBUG_STATISTICS
drwnFactor::_dbStatsCurrentMem -= _nSize;
#endif
}
// load and add variables
drwnFactor::load(xml);
// read table data
drwnXMLNode *node = xml.first_node("data");
if ((node == NULL) || drwnIsXMLEmpty(*node)) {
initialize();
} else {
VectorXd v;
drwnXMLUtils::deserialize(*node, v);
DRWN_ASSERT_MSG(v.size() == _nSize, v.size() << " != " << _nSize);
Eigen::Map<VectorXd>(_data, _nSize) = v;
}
return true;
}
void GenerateLambdas(vector<VectorXd>& allLams, vector<VectorXd>& allZscores, double minNCP){
for(unsigned int i =0; i < allZscores.size(); i++){
VectorXd zTemp =allZscores[i];
VectorXd lamI(zTemp.size());
for(int j =0; j <zTemp.size(); j++){
if(zTemp[j]>0){
if(zTemp[j] < minNCP){
lamI[j] = minNCP;
}
else{
lamI[j] = zTemp[j];
}
}
else{
if(zTemp[j] > -1*minNCP){
lamI[j] = -1*minNCP;
}
else{
lamI[j] = zTemp[j];
}
}
}
allLams.push_back(lamI);
}
}
void LoadMatrices(MatrixXd &A, VectorXd &W, VectorXd &p, string weight_type) {
A = MatrixXd::Zero(n_n, n_n);
MatrixXd Apn = MatrixXd::Zero(n_p, n_n);
MatrixXd abs_Apn = MatrixXd::Zero(n_p, n_n);
VectorXd k = VectorXd::Zero(n_n);
p = VectorXd::Zero(n_n);
W = VectorXd::Zero(n_p);
// Topological incidence matrix
for (unsigned int j = 0; j < n_p; j++) {
int idx_n1 = wds->agelemek.at(pipe_idx.at(j))->Get_Cspe_Index();
int idx_n2 = wds->agelemek.at(pipe_idx.at(j))->Get_Cspv_Index();
Apn(j, idx_n1)++;
Apn(j, idx_n2)--;
abs_Apn(j, idx_n1)++;
abs_Apn(j, idx_n2)++;
}
A = Apn.transpose() * Apn;
for (unsigned i = 0; i < n_n; i++) {
A(i, i) = 0;
for (unsigned int j = 0; j < n_n; j++) {
if (A(i, j) != 0) {
A(i, j) = 1.;
// k(i)++;
}
}
}
if (0 == strcmp(weight_type.c_str(), "topology")) {
for (unsigned int i = 0; i < W.size(); i++)
W(i) = 1;
}
if (0 == strcmp(weight_type.c_str(), "dp")) {
double dp, dp_max = -1., weight_min = 0.0001;
for (unsigned int i = 0; i < W.size(); i++) {
// dp = wds->agelemek.at(pipe_idx.at(i))->Get_dprop("mass_flow_rate");
dp = wds->agelemek.at(pipe_idx.at(i))->Get_dprop("headloss");
// dp = wds->agelemek.at(pipe_idx.at(i))->Get_dprop("length");
W(i) = abs(dp);
if (W(i) > dp_max)
dp_max = W(i);
//cout<<endl<<wds->agelemek.at(pipe_idx.at(i))->Get_nev()<<" dp="<<dp;
}
for (unsigned int i = 0; i < W.size(); i++) {
W(i) /= dp_max;
if (W(i) < weight_min)
W(i) = weight_min;
// cout<<endl<<wds->agelemek.at(pipe_idx.at(i))->Get_nev()<<" weight="<<W(i);
}
}
// Final computations
p = abs_Apn.transpose() * W;
sumW = W.sum();
}
VectorXd RungeKuttaEOM(VectorXd Q, double t)
{
VectorXd Qd(Q.size());
Qd.block(0, 0, Q.size()/2, 1) = m_qd;
Qd.block(Q.size()/2, 0, Q.size()/2, 1) = m_qdd;
return Qd;
}
int operator()(const VectorXd &b, VectorXd &fvec) const {
ASSERT(b.size() == unknowns);
ASSERT(fvec.size() == datasetLen);
for(int i = 0; i < datasetLen; i++) {
double x = _x[i], xx = x*x, xxx = xx*x;
fvec[i] = (b[0] + b[1]*x + b[2]*xx + b[3]*xxx)/(1. + b[4]*x + b[5]*xx + b[6]*xxx) - _y[i];
}
return 0;
}
int operator()(const VectorXd &b, VectorXd &fvec) const {
ASSERT(b.size() == unknowns);
ASSERT(fvec.size() == datasetLen);
for (int i = 0; i < unknowns; ++i)
(*fSource).SetCoeffVal(i, b(i));
for(int64 i = 0; i < datasetLen; i++)
fvec(ptrdiff_t(i)) = (*fSource).f((*series).x(i)) - (*series).y(i);
return 0;
}
/* ********************************************************************************************* */
inline void saveLine (char* l1, char* l2, size_t off, const VectorXd& n1, const VectorXd& n2) {
if(n1.size() == 2) {
sprintf(l1 + off, "%+02.3lf %+02.3lf ", n1(0), n1(1));
sprintf(l2 + off, "%+02.3lf %+02.3lf ", n2(0), n2(1));
}
else if(n1.size() == 3) {
sprintf(l1 + off, "%+02.3lf %+02.3lf %+02.3lf ", n1(0), n1(1), n1(2));
sprintf(l2 + off, "%+02.3lf %+02.3lf %+02.3lf ", n2(0), n2(1), n2(2));
}
}
double compute_cost(Vector4d beta, VectorXd x, VectorXd y){
double cost;
assert(x.size()==y.size());
ArrayXd pred, cost_array;
pred = model_fun(beta, x);
cost_array = (pred - y.array()).pow(2);
cost = cost_array.sum() / (2.*x.size());
return cost;
}
VectorXd Zscores2Post(VectorXd& Zs){
VectorXd post(Zs.size());
VectorXd Zsq = Zs.array().square();
for(int i = 0; i < Zsq.size(); i ++){
VectorXd Ztemp = (Zsq.array() - Zsq[i])/2;
VectorXd Zexp = Ztemp.array().exp();
post[i] = 1/Zexp.sum();
}
return(post);
}
UnscentedTransform::UnscentedTransform (VectorXd x, MatrixXd Px, ITransformer* transformer)
{
// validation of params
if (x.size() != Px.rows() || x.size() != Px.cols())
throw std::length_error("Initialization of Unscented Transform failed. "
"Invalid size of covariance, doesn't match that of vector x.");
if (transformer == 0)
throw std::runtime_error("Initialization of Unscented Transform failed. "
"No transformer passed.");
// initializes parameters
this->x = x;
this->Px = Px;
this->transformer = transformer;
// set default scaling factors
alpha = 1; // spread
beta = 2; // distribution = Gaussian
kappa = 3 - x.size();
// initialize output; to get the size of y, we must propagate a
// vector through the transformer
VectorXd testY = transformer->transform(VectorXd::Zero(x.size()));
y = VectorXd::Zero(testY.size());
Py = MatrixXd::Zero(testY.size(),testY.size());
Pxy = MatrixXd::Zero(x.size(),testY.size());
locked = false; // compute() not called yet
}
void NewPost_chol(VectorXd& Marginal, vector<VectorXd>& Zs, vector<VectorXd>& Lams, VectorXd& beta, MatrixXd& Aj, vector<MatrixXd>& upper_chol, int NC, double& fullLikeli){
int numsnps = Zs[0].size();
int num_pops = Zs.size();
double runsum = 0;
for(int i =0 ; i < Marginal.size(); i++){
Marginal(i) = -1e150;
}
double c_prob = 0;
double z_prob = 0;
double sum = 0;
VectorXd causConfig(numsnps);
causConfig.setZero();
VectorXd causIndex(NC);
causIndex.setZero();
VectorXd evalPrior(numsnps);
EvalAijs(Aj, beta, evalPrior);
c_prob = Prob_CNew(evalPrior, beta, causConfig);
double z_pop_prob = 0;
for (int i = 0; i<num_pops ; i++) {
z_pop_prob=EvaluateLogMvn_Cholesky(Zs[i], causConfig, Lams[i], upper_chol[i]);
z_prob += z_pop_prob;
}
sum = c_prob+z_prob;
runsum = sum;
int counter = 1;
while(NextCombo(causIndex, NC, numsnps+1) == 1){
BuildCausalVector(causConfig, causIndex);
c_prob = Prob_CNew(evalPrior, beta, causConfig);
z_prob = 0;
for (int i = 0; i<num_pops ; i++) {
z_pop_prob=EvaluateLogMvn_Cholesky(Zs[i], causConfig, Lams[i], upper_chol[i]);
z_prob += z_pop_prob;
}
sum = c_prob+z_prob;
runsum = LogSum(runsum, sum);
for(int j = 0; j < causIndex.size(); j++){
if(causIndex[j] >0){
Marginal[causIndex[j]-1] = LogSum(Marginal[causIndex[j]-1], sum);
}
}
counter ++;
}
for(int f = 0 ; f < Marginal.size(); f++){
Marginal[f] = Marginal[f]- runsum;
}
fullLikeli = fullLikeli+runsum;
}
void doConjStep(VectorXd &XY,
VectorXd &s0,
VectorXd &gradE,
const vector<spring> &springlist,
const vector<vector<int>> &springpairs,
int bendingOn,
double kappa,
int conjsteps,
double g11,
double g12,
double g22)
{
double a1=0.0;
double a2=1.0;
double betan;
VectorXd gradEn(gradE.size());
VectorXd sn(s0.size());
functor network(XY,s0,springlist,springpairs,kappa,g11,g12,g22);
doBracketfind(a1,a2,network);
if(doBracketfind(a1,a2,network))
{
// double an=doFalsePosition(network,a1,a2);
//double an=Ridder(network,a1,a2);
double an=Brent(network,a1,a2,1e-12);
//Update the positions.
XY=XY+an*s0;
if(bendingOn==0){
gradEn=HarmonicGradient(springlist,XY,g11,g12,g22);
} else{
gradEn=HarmonicGradient(springlist,XY,g11,g12,g22)+BendingGrad(springpairs,springlist,XY,kappa,g11,g12,g22);
}
betan=(gradEn-gradE).dot(gradEn)/(gradE.dot(gradE));
//Did not find bracket, reset CG-method
} else{
betan=0.0;
if(bendingOn==0){
gradE=HarmonicGradient(springlist,XY,g11,g12,g22);
} else{
gradE=HarmonicGradient(springlist,XY,g11,g12,g22)+BendingGrad(springpairs,springlist,XY,kappa,g11,g12,g22);
}
s0=-gradE;
//cout<<"Bracket failed, Reset CG"<<endl;
return;
}
if(conjsteps%5 ==0.0) betan=0.0;
if(betan<0.0) betan=0; //max(betan,0)
if(abs(gradEn.dot(gradE))>.5*gradE.dot(gradE)) betan=0.0;
if(-2*gradE.dot(gradE)>gradE.dot(s0) && gradE.dot(s0) >-.2*gradE.dot(gradE)) betan=0.0;
//cout<<"\r"<<"** Beta "<<betan<<flush;
sn=-gradEn+betan*s0;
gradE=gradEn;
s0=sn;
}
void LeastSquareFitClass::make1dPolynomialMatrix(MatrixXd *P,
VectorXd x,
int polynomialDegree){
P->resize(x.size(),polynomialDegree+1);
for (int iCol = 0; iCol < polynomialDegree+1; iCol++){
for (int iRow = 0; iRow < x.size(); iRow++){
(*P)(iRow,iCol) = pow(x(iRow),iCol);
}
}
}
int argmax(const VectorXd &v)
{
if (v.size() == 0)
return -1;
int maxInd = 0;
for (int i = 1; i < (int)v.size(); i++) {
if (v[i] > v[maxInd])
maxInd = i;
}
return maxInd;
}