void
PatchSample::sample_frustum(double hfov, double vfov, int nh, int nv,
                            Vector3d p, Vector3d u,
                            MatrixXd &mx, MatrixXd &my, MatrixXd &mz)
{
  //TBD: check input args

  // pointing, up, left
  p = p/p.norm();
  u = u/u.norm();
  Vector3d l = u.cross(p);

  // sample like a camera would
  double ll = (tan(hfov/2.0)*((double)(nh-1)))/nh;
  double uu = (tan(vfov/2.0)*((double)(nv-1)))/nv;
  RowVectorXd y;
  y.setLinSpaced(nh,-ll,ll);
  MatrixXd yy;
  yy = y.replicate(nv,1);
  VectorXd z;
  z.setLinSpaced(nv,-uu,uu);
  MatrixXd zz;
  zz = z.replicate(1,nh);
  MatrixXd xx = MatrixXd::Ones(nv,nh);
  MatrixXd nn = (xx.array().square() + yy.array().square() + zz.array().square()).cwiseSqrt();
  xx = xx.array() / nn.array();
  yy = yy.array() / nn.array();
  zz = zz.array() / nn.array();

  // rotation matrix
  Matrix3d rr;
  rr << p, l, u;

  // rotate points
  MatrixXd xyz;
  MatrixXd cam (3,xx.rows()*xx.cols());
  cam.row(0) = vectorizeColWise(xx);
  cam.row(1) = vectorizeColWise(yy);
  cam.row(2) = vectorizeColWise(zz);
  xyz = rr*cam;

  // extract coordinates
  xx = xyz.row(0);
  yy = xyz.row(1);
  zz = xyz.row(2);
  mx = Map<MatrixXd>(xx.data(),nv,nh);
  my = Map<MatrixXd>(yy.data(),nv,nh);
  mz = Map<MatrixXd>(zz.data(),nv,nh);
}
Exemple #2
0
void EEMS::save_iteration(const MCMC &mcmc) {
  int iter = mcmc.to_save_iteration( );
  mcmcthetas(iter,0) = nowsigma2;
  mcmcthetas(iter,1) = nowdf;
  mcmcqhyper(iter,0) = 0.0;
  mcmcqhyper(iter,1) = nowqrateS2;
  mcmcmhyper(iter,0) = nowmrateMu;
  mcmcmhyper(iter,1) = nowmrateS2;
  mcmcpilogl(iter,0) = nowpi;
  mcmcpilogl(iter,1) = nowll;
  mcmcqtiles(iter) = nowqtiles;
  mcmcmtiles(iter) = nowmtiles;
  for ( int t = 0 ; t < nowqtiles ; t++ ) {
    mcmcqRates.push_back(pow(10.0,nowqEffcts(t)));
  }
  for ( int t = 0 ; t < nowqtiles ; t++ ) {
    mcmcwCoord.push_back(nowqSeeds(t,0));
  }
  for ( int t = 0 ; t < nowqtiles ; t++ ) {
    mcmczCoord.push_back(nowqSeeds(t,1));
  }
  for ( int t = 0 ; t < nowmtiles ; t++ ) {
    mcmcmRates.push_back(pow(10.0,nowmEffcts(t) + nowmrateMu));
  }
  for ( int t = 0 ; t < nowmtiles ; t++ ) {
    mcmcxCoord.push_back(nowmSeeds(t,0));
  }
  for ( int t = 0 ; t < nowmtiles ; t++ ) {
    mcmcyCoord.push_back(nowmSeeds(t,1));
  }
  MatrixXd B = nowBinv.inverse();
  VectorXd h = B.diagonal();
  B -= 0.5 * h.replicate(1,o);
  B -= 0.5 * h.transpose().replicate(o,1);
  B += 0.5 * nowW.replicate(1,o);
  B += 0.5 * nowW.transpose().replicate(o,1);
  JtDhatJ += nowsigma2 * B;
}
Exemple #3
0
int main()
{
  RigidBodyManipulator rbm("examples/Atlas/urdf/atlas_minimal_contact.urdf");
  RigidBodyManipulator* model = &rbm;

  if(!model)
  {
    cerr<<"ERROR: Failed to load model"<<endl;
  }
  Vector2d tspan;
  tspan<<0,1;
  int l_hand;
  int r_hand;
  //int l_foot;
  //int r_foot;
  for(int i = 0;i<model->bodies.size();i++)
  {
    if(model->bodies[i]->linkname.compare(string("l_hand")))
    {
      l_hand = i;
    }
    else if(model->bodies[i]->linkname.compare(string("r_hand")))
    {
      r_hand = i;
    }
    //else if(model->bodies[i].linkname.compare(string("l_foot")))
    //{
    //  l_foot = i;
    //}
    //else if(model->bodies[i].linkname.compare(string("r_foot")))
    //{
    //  r_foot = i;
    //}
  }
  int nq = model->num_positions;
  VectorXd qstar = VectorXd::Zero(nq);
  qstar(3) = 0.8;
  KinematicsCache<double> cache = model->doKinematics(qstar, 0);
  Vector3d com0 = model->centerOfMass(cache, 0).value();

  Vector3d r_hand_pt = Vector3d::Zero();
  Vector3d rhand_pos0 = model->forwardKin(cache, r_hand_pt, r_hand, 0, 0, 0).value();

  int nT = 4;
  double* t = new double[nT];
  double dt = 1.0/(nT-1);
  for(int i = 0;i<nT;i++)
  {
    t[i] = dt*i;
  }
  MatrixXd q0 = qstar.replicate(1,nT);
  VectorXd qdot0 = VectorXd::Zero(model->num_velocities);
  Vector3d com_lb = com0;
  com_lb(0) = std::numeric_limits<double>::quiet_NaN();
  com_lb(1) = std::numeric_limits<double>::quiet_NaN();
  Vector3d com_ub = com0;
  com_ub(0) = std::numeric_limits<double>::quiet_NaN();
  com_ub(1) = std::numeric_limits<double>::quiet_NaN();
  com_ub(2) = com0(2)+0.5;
  WorldCoMConstraint* com_kc = new WorldCoMConstraint(model,com_lb,com_ub);
  Vector3d rhand_pos_lb = rhand_pos0;
  rhand_pos_lb(0) +=0.1;
  rhand_pos_lb(1) +=0.05;
  rhand_pos_lb(2) +=0.25;
  Vector3d rhand_pos_ub = rhand_pos_lb;
  rhand_pos_ub(2) += 0.25;
  Vector2d tspan_end;
  tspan_end<<t[nT-1],t[nT-1];
  WorldPositionConstraint* kc_rhand = new WorldPositionConstraint(model,r_hand,r_hand_pt,rhand_pos_lb,rhand_pos_ub,tspan_end);
  int num_constraints = 2;
  RigidBodyConstraint** constraint_array = new RigidBodyConstraint*[num_constraints];
  constraint_array[0] = com_kc;
  constraint_array[1] = kc_rhand;
  IKoptions ikoptions(model);
  MatrixXd q_sol(model->num_positions,nT);
  MatrixXd qdot_sol(model->num_velocities,nT);
  MatrixXd qddot_sol(model->num_positions,nT);
  int info = 0;
  vector<string> infeasible_constraint;
  inverseKinTraj(model,nT,t,qdot0,q0,q0,num_constraints,constraint_array,q_sol,qdot_sol,qddot_sol,info,infeasible_constraint,ikoptions);
  printf("INFO = %d\n",info);
  if(info != 1)
  {
    cerr<<"Failure"<<endl;
    return 1;
  }
  ikoptions.setFixInitialState(false);
  ikoptions.setMajorIterationsLimit(500);
  inverseKinTraj(model,nT,t,qdot0,q0,q0,num_constraints,constraint_array,q_sol,qdot_sol,qddot_sol,info,infeasible_constraint,ikoptions);
  printf("INFO = %d\n",info);
  if(info != 1)
  {
    cerr<<"Failure"<<endl;
    return 1;
  }
  RowVectorXd t_inbetween(5);
  t_inbetween << 0.1,0.15,0.3,0.4,0.6;
  ikoptions.setAdditionaltSamples(t_inbetween);
  inverseKinTraj(model,nT,t,qdot0,q0,q0,num_constraints,constraint_array,q_sol,qdot_sol,qddot_sol,info,infeasible_constraint,ikoptions);
  printf("INFO = %d\n",info);
  if(info != 1)
  {
    cerr<<"Failure"<<endl;
    return 1;
  }
  delete com_kc;
  delete[] constraint_array;
  delete[] t;
  return 0;
}
Exemple #4
0
int main(int argc, char *argv[])
{
  using namespace Eigen;
  using namespace std;

  // Load a mesh in OFF format
  igl::readOFF("../shared/cow.off", V, F);

  // Compute Laplace-Beltrami operator: #V by #V
  igl::cotmatrix(V,F,L);

  // Alternative construction of same Laplacian
  SparseMatrix<double> G,K;
  // Gradient/Divergence
  igl::grad(V,F,G);
  // Diagonal per-triangle "mass matrix"
  VectorXd dblA;
  igl::doublearea(V,F,dblA);
  // Place areas along diagonal #dim times
  const auto & T = 1.*(dblA.replicate(3,1)*0.5).asDiagonal();
  // Laplacian K built as discrete divergence of gradient or equivalently
  // discrete Dirichelet energy Hessian
  K = -G.transpose() * T * G;
  cout<<"|K-L|: "<<(K-L).norm()<<endl;

  const auto &key_down = [](igl::Viewer &viewer,unsigned char key,int mod)->bool
  {
    switch(key)
    {
      case 'r':
      case 'R':
        U = V;
        break;
      case ' ':
      {
        // Recompute just mass matrix on each step
        SparseMatrix<double> M;
        igl::massmatrix(U,F,igl::MASSMATRIX_TYPE_BARYCENTRIC,M);
        // Solve (M-delta*L) U = M*U
        const auto & S = (M - 0.001*L);
        Eigen::SimplicialLLT<Eigen::SparseMatrix<double > > solver(S);
        assert(solver.info() == Eigen::Success);
        U = solver.solve(M*U).eval();
        // Compute centroid and subtract (also important for numerics)
        VectorXd dblA;
        igl::doublearea(U,F,dblA);
        double area = 0.5*dblA.sum();
        MatrixXd BC;
        igl::barycenter(U,F,BC);
        RowVector3d centroid(0,0,0);
        for(int i = 0;i<BC.rows();i++)
        {
          centroid += 0.5*dblA(i)/area*BC.row(i);
        }
        U.rowwise() -= centroid;
        // Normalize to unit surface area (important for numerics)
        U.array() /= sqrt(area);
        break;
      }
      default:
        return false;
    }
    // Send new positions, update normals, recenter
    viewer.data.set_vertices(U);
    viewer.data.compute_normals();
    viewer.core.align_camera_center(U,F);
    return true;
  };


  // Use original normals as pseudo-colors
  MatrixXd N;
  igl::per_vertex_normals(V,F,N);
  MatrixXd C = N.rowwise().normalized().array()*0.5+0.5;

  // Initialize smoothing with base mesh
  U = V;
  viewer.data.set_mesh(U, F);
  viewer.data.set_colors(C);
  viewer.callback_key_down = key_down;

  cout<<"Press [space] to smooth."<<endl;;
  cout<<"Press [r] to reset."<<endl;;
  return viewer.launch();
}
Exemple #5
0
int main()
{
  URDFRigidBodyManipulator* model = loadURDFfromFile("../../examples/Atlas/urdf/atlas_minimal_contact.urdf");
  if(!model)
  {
    cerr<<"ERROR: Failed to load model"<<endl;
  }
  Vector2d tspan;
  tspan<<0,1;
  int l_hand;
  int r_hand;
  //int l_foot;
  //int r_foot;
  for(int i = 0;i<model->num_bodies;i++)
  {
    if(model->bodies[i].linkname.compare(string("l_hand")))
    {
      l_hand = i;
    }
    else if(model->bodies[i].linkname.compare(string("r_hand")))
    {
      r_hand = i;
    }
    //else if(model->bodies[i].linkname.compare(string("l_foot")))
    //{
    //  l_foot = i;
    //}
    //else if(model->bodies[i].linkname.compare(string("r_foot")))
    //{
    //  r_foot = i;
    //}
  }
  int nq = model->num_dof;
  VectorXd qstar = VectorXd::Zero(nq);
  qstar(3) = 0.8;
  model->doKinematics(qstar.data());
  Vector3d com0;
  model->getCOM(com0);
  Vector4d l_hand_pt;
  l_hand_pt<<0.0,0.0,0.0,1.0;
  Vector4d r_hand_pt;
  r_hand_pt<<0.0,0.0,0.0,1.0;
  Vector3d lhand_pos0;
  model->forwardKin(l_hand,l_hand_pt,0,lhand_pos0);
  Vector3d rhand_pos0;
  model->forwardKin(r_hand,r_hand_pt,0,rhand_pos0);
  int nT = 4;
  double* t = new double[nT];
  double dt = 1.0/(nT-1);
  for(int i = 0;i<nT;i++)
  {
    t[i] = dt*i;
  }
  MatrixXd q0 = qstar.replicate(1,nT);
  VectorXd qdot0 = VectorXd::Zero(model->num_dof);
  Vector3d com_lb = com0;
  com_lb(0) = nan("");;
  com_lb(1) = nan("");
  Vector3d com_ub = com0;
  com_ub(0) = nan("");
  com_ub(1) = nan("");
  com_ub(2) = com0(2)+0.5;
  WorldCoMConstraint* com_kc = new WorldCoMConstraint(model,com_lb,com_ub);
  Vector3d rhand_pos_lb = rhand_pos0;
  rhand_pos_lb(0) +=0.1;
  rhand_pos_lb(1) +=0.05;
  rhand_pos_lb(2) +=0.25;
  Vector3d rhand_pos_ub = rhand_pos_lb;
  rhand_pos_ub(2) += 0.25;
  Vector2d tspan_end;
  tspan_end<<t[nT-1],t[nT-1];
  WorldPositionConstraint* kc_rhand = new WorldPositionConstraint(model,r_hand,r_hand_pt,rhand_pos_lb,rhand_pos_ub,tspan_end);
  int num_constraints = 2;
  RigidBodyConstraint** constraint_array = new RigidBodyConstraint*[num_constraints];
  constraint_array[0] = com_kc;
  constraint_array[1] = kc_rhand;
  IKoptions ikoptions(model);
  MatrixXd q_sol(model->num_dof,nT);
  MatrixXd qdot_sol(model->num_dof,nT);
  MatrixXd qddot_sol(model->num_dof,nT);
  int info = 0;
  vector<string> infeasible_constraint;
  inverseKinTraj(model,nT,t,qdot0,q0,q0,num_constraints,constraint_array,q_sol,qdot_sol,qddot_sol,info,infeasible_constraint,ikoptions);
  printf("INFO = %d\n",info);
  if(info != 1)
  {
    cerr<<"Failure"<<endl;
    return 1;
  }
  ikoptions.setFixInitialState(false);
  ikoptions.setMajorIterationsLimit(500);
  inverseKinTraj(model,nT,t,qdot0,q0,q0,num_constraints,constraint_array,q_sol,qdot_sol,qddot_sol,info,infeasible_constraint,ikoptions);
  printf("INFO = %d\n",info);
  if(info != 1)
  {
    cerr<<"Failure"<<endl;
    return 1;
  }
  RowVectorXd t_inbetween(5);
  t_inbetween << 0.1,0.15,0.3,0.4,0.6;
  ikoptions.setAdditionaltSamples(t_inbetween);
  inverseKinTraj(model,nT,t,qdot0,q0,q0,num_constraints,constraint_array,q_sol,qdot_sol,qddot_sol,info,infeasible_constraint,ikoptions);
  printf("INFO = %d\n",info);
  if(info != 1)
  {
    cerr<<"Failure"<<endl;
    return 1;
  }
  delete com_kc;
  delete[] constraint_array;
  delete[] t;
  return 0;
}
Exemple #6
0
CFeatures* CJade::apply(CFeatures* features)
{
	ASSERT(features);
	SG_REF(features);

	SGMatrix<float64_t> X = ((CDenseFeatures<float64_t>*)features)->get_feature_matrix();

	int n = X.num_rows;
	int T = X.num_cols;
	int m = n;

	Map<MatrixXd> EX(X.matrix,n,T);

	// Mean center X
	VectorXd mean = (EX.rowwise().sum() / (float64_t)T);
	MatrixXd SPX = EX.colwise() - mean;

	MatrixXd cov = (SPX * SPX.transpose()) / (float64_t)T;

	#ifdef DEBUG_JADE
	std::cout << "cov" << std::endl;
	std::cout << cov << std::endl;
	#endif

	// Whitening & Projection onto signal subspace
	SelfAdjointEigenSolver<MatrixXd> eig;
	eig.compute(cov);

	#ifdef DEBUG_JADE
	std::cout << "eigenvectors" << std::endl;
	std::cout << eig.eigenvectors() << std::endl;

	std::cout << "eigenvalues" << std::endl;
	std::cout << eig.eigenvalues().asDiagonal() << std::endl;
	#endif

	// Scaling
	VectorXd scales = eig.eigenvalues().cwiseSqrt();
	MatrixXd B = scales.cwiseInverse().asDiagonal() * eig.eigenvectors().transpose();

	#ifdef DEBUG_JADE
	std::cout << "whitener" << std::endl;
	std::cout << B << std::endl;
	#endif

	// Sphering
	SPX = B * SPX;

	// Estimation of the cumulant matrices
	int dimsymm = (m * ( m + 1)) / 2; // Dim. of the space of real symm matrices
	int nbcm = dimsymm; //  number of cumulant matrices
	m_cumulant_matrix = SGMatrix<float64_t>(m,m*nbcm);	// Storage for cumulant matrices
	Map<MatrixXd> CM(m_cumulant_matrix.matrix,m,m*nbcm);
	MatrixXd R(m,m); R.setIdentity();
	MatrixXd Qij = MatrixXd::Zero(m,m); // Temp for a cum. matrix
	VectorXd Xim = VectorXd::Zero(m); // Temp
	VectorXd Xjm = VectorXd::Zero(m); // Temp
	VectorXd Xijm = VectorXd::Zero(m); // Temp
	int Range = 0;

	for (int im = 0; im < m; im++)
	{
		Xim = SPX.row(im);
		Xijm = Xim.cwiseProduct(Xim);
		Qij = SPX.cwiseProduct(Xijm.replicate(1,m).transpose()) * SPX.transpose() / (float)T - R - 2*R.col(im)*R.col(im).transpose();
		CM.block(0,Range,m,m) = Qij;
		Range = Range + m;
		for (int jm = 0; jm < im; jm++)
		{
			Xjm = SPX.row(jm);
			Xijm = Xim.cwiseProduct(Xjm);
			Qij = SPX.cwiseProduct(Xijm.replicate(1,m).transpose()) * SPX.transpose() / (float)T - R.col(im)*R.col(jm).transpose() - R.col(jm)*R.col(im).transpose();
			CM.block(0,Range,m,m) =  sqrt(2)*Qij;
			Range = Range + m;
		}
	}

	#ifdef DEBUG_JADE
	std::cout << "cumulatant matrices" << std::endl;
	std::cout << CM << std::endl;
	#endif

	// Stack cumulant matrix into ND Array
	index_t * M_dims = SG_MALLOC(index_t, 3);
	M_dims[0] = m;
	M_dims[1] = m;
	M_dims[2] = nbcm;
	SGNDArray< float64_t > M(M_dims, 3);

	for (int i = 0; i < nbcm; i++)
	{
		Map<MatrixXd> EM(M.get_matrix(i),m,m);
		EM = CM.block(0,i*m,m,m);
	}

	// Diagonalize
	SGMatrix<float64_t> Q = CJADiagOrth::diagonalize(M, m_mixing_matrix, tol, max_iter);
	Map<MatrixXd> EQ(Q.matrix,m,m);
	EQ = -1 * EQ.inverse();

	#ifdef DEBUG_JADE
	std::cout << "diagonalizer" << std::endl;
	std::cout << EQ << std::endl;
	#endif

	// Separating matrix
	SGMatrix<float64_t> sep_matrix = SGMatrix<float64_t>(m,m);
	Map<MatrixXd> C(sep_matrix.matrix,m,m);
	C = EQ.transpose() * B;

	// Sort
	VectorXd A = (B.inverse()*EQ).cwiseAbs2().colwise().sum();
	bool swap = false;
	do
	{
		swap = false;
		for (int j = 1; j < n; j++)
		{
			if ( A(j) < A(j-1) )
			{
				std::swap(A(j),A(j-1));
				C.col(j).swap(C.col(j-1));
				swap = true;
			}
		}

	} while(swap);

	for (int j = 0; j < m/2; j++)
		C.row(j).swap( C.row(m-1-j) );

	// Fix Signs
	VectorXd signs = VectorXd::Zero(m);
	for (int i = 0; i < m; i++)
	{
		if( C(i,0) < 0 )
			signs(i) = -1;
		else
			signs(i) = 1;
	}
	C = signs.asDiagonal() * C;

	#ifdef DEBUG_JADE
	std::cout << "un mixing matrix" << std::endl;
	std::cout << C << std::endl;
	#endif

	// Unmix
	EX = C * EX;

	m_mixing_matrix = SGMatrix<float64_t>(m,m);
	Map<MatrixXd> Emixing_matrix(m_mixing_matrix.matrix,m,m);
	Emixing_matrix = C.inverse();

	return features;
}
Exemple #7
0
GTEST_TEST(testIKtraj, testIKtraj) {
  RigidBodyTree model(
      GetDrakePath() + "/examples/Atlas/urdf/atlas_minimal_contact.urdf");

  int r_hand{};
  int pelvis{};
  for (int i = 0; i < static_cast<int>(model.bodies.size()); i++) {
    if (!model.bodies[i]->get_name().compare(std::string("r_hand"))) {
      r_hand = i;
    }
    if (!model.bodies[i]->get_name().compare(std::string("pelvis"))) {
      pelvis = i;
    }
  }

  VectorXd qstar = model.getZeroConfiguration();
  qstar(3) = 0.8;
  KinematicsCache<double> cache = model.doKinematics(qstar);
  Vector3d com0 = model.centerOfMass(cache);

  Vector3d r_hand_pt = Vector3d::Zero();
  Vector3d rhand_pos0 = model.transformPoints(cache, r_hand_pt, r_hand, 0);

  Vector2d tspan(0, 1);
  int nT = 4;
  double dt = tspan(1) / (nT - 1);
  std::vector<double> t(nT, 0);
  for (int i = 0; i < nT; i++) {
    t[i] = dt * i;
  }
  MatrixXd q0 = qstar.replicate(1, nT);
  VectorXd qdot0 = VectorXd::Zero(model.get_num_velocities());
  Vector3d com_lb = com0;
  com_lb(0) = std::numeric_limits<double>::quiet_NaN();
  com_lb(1) = std::numeric_limits<double>::quiet_NaN();
  Vector3d com_ub = com0;
  com_ub(0) = std::numeric_limits<double>::quiet_NaN();
  com_ub(1) = std::numeric_limits<double>::quiet_NaN();
  com_ub(2) = com0(2) + 0.5;
  WorldCoMConstraint com_kc(&model, com_lb, com_ub);
  Vector3d rhand_pos_lb = rhand_pos0;
  rhand_pos_lb(0) += 0.1;
  rhand_pos_lb(1) += 0.05;
  rhand_pos_lb(2) += 0.25;
  Vector3d rhand_pos_ub = rhand_pos_lb;
  rhand_pos_ub(2) += 0.25;
  Vector2d tspan_end;
  tspan_end << t[nT - 1], t[nT - 1];
  WorldPositionConstraint kc_rhand(
      &model, r_hand, r_hand_pt, rhand_pos_lb, rhand_pos_ub, tspan_end);

  // Add a multiple time constraint which is fairly trivial to meet in
  // this case.
  WorldFixedBodyPoseConstraint kc_fixed_pose(&model, pelvis, tspan);

  std::vector<RigidBodyConstraint*> constraint_array;
  constraint_array.push_back(&com_kc);
  constraint_array.push_back(&kc_rhand);
  constraint_array.push_back(&kc_fixed_pose);

  IKoptions ikoptions(&model);
  MatrixXd q_sol(model.get_num_positions(), nT);
  MatrixXd qdot_sol(model.get_num_velocities(), nT);
  MatrixXd qddot_sol(model.get_num_positions(), nT);
  int info = 0;
  std::vector<std::string> infeasible_constraint;

  inverseKinTraj(&model, nT, t.data(), qdot0, q0, q0, constraint_array.size(),
                 constraint_array.data(), ikoptions, &q_sol, &qdot_sol,
                 &qddot_sol, &info, &infeasible_constraint);
  EXPECT_EQ(info, 1);

  ikoptions.setFixInitialState(false);
  ikoptions.setMajorIterationsLimit(500);

  inverseKinTraj(&model, nT, t.data(), qdot0, q0, q0, constraint_array.size(),
                 constraint_array.data(), ikoptions, &q_sol, &qdot_sol,
                 &qddot_sol, &info, &infeasible_constraint);
  EXPECT_EQ(info, 1);

  Eigen::RowVectorXd t_inbetween(5);
  t_inbetween << 0.1, 0.15, 0.3, 0.4, 0.6;
  ikoptions.setAdditionaltSamples(t_inbetween);
  inverseKinTraj(&model, nT, t.data(), qdot0, q0, q0, constraint_array.size(),
                 constraint_array.data(), ikoptions, &q_sol, &qdot_sol,
                 &qddot_sol, &info, &infeasible_constraint);
  EXPECT_EQ(info, 1);
}
void UpdaterCovarAdaptation::updateDistribution(const DistributionGaussian& distribution, const MatrixXd& samples, const VectorXd& costs, VectorXd& weights, DistributionGaussian& distribution_new) const
{
  int n_samples = samples.rows();
  int n_dims = samples.cols();
  
  VectorXd mean_cur = distribution.mean();
  assert(mean_cur.size()==n_dims);
  assert(costs.size()==n_samples);
  
  // Update the mean
  VectorXd mean_new;
  updateDistributionMean(mean_cur, samples, costs, weights, mean_new); 
  distribution_new.set_mean(mean_new);

  
  
  // Update the covariance matrix with reward-weighted averaging
  MatrixXd eps = samples - mean_cur.transpose().replicate(n_samples,1);
  // In Matlab: covar_new = (repmat(weights,1,n_dims).*eps)'*eps;
  MatrixXd weighted_eps = weights.replicate(1,n_dims).array()*eps.array();
  MatrixXd covar_new = weighted_eps.transpose()*eps;

  //MatrixXd summary(n_samples,2*n_dims+2);
  //summary << samples, eps, costs, weights;
  //cout << fixed << setprecision(2);
  //cout << summary << endl;

  // Remove non-diagonal values
  if (diag_only_) {
    MatrixXd diagonalized = covar_new.diagonal().asDiagonal();
    covar_new = diagonalized;    
  }
  
  // Low-pass filter for covariance matrix, i.e. weight between current and new covariance matrix.
  if (learning_rate_<1.0) {
    MatrixXd covar_cur = distribution.covar();
    covar_new = (1-learning_rate_)*covar_cur + learning_rate_*covar_new;
  }
  
  // Add a base_level to avoid pre-mature convergence
  if (base_level_diagonal_.size()>0) // If base_level is empty, do nothing
  {
    assert(base_level_diagonal_.size()==n_dims);
    for (int ii=0; ii<covar_new.rows(); ii++)
      if (covar_new(ii,ii)<base_level_diagonal_(ii))
        covar_new(ii,ii) = base_level_diagonal_(ii);
  }
  
  if (relative_lower_bound_>0.0)
  {
    // We now enforce a lower bound on the eigenvalues, that depends on the maximum eigenvalue. For
    // instance, if max(eigenvalues) is 2 and relative_lower_bound is 0.2, none of the eigenvalues
    // may be below 0.4.
    SelfAdjointEigenSolver<MatrixXd> eigensolver(covar_new);
    if (eigensolver.info() == Success)
    {
      // Get the eigenvalues
      VectorXd eigen_values  = eigensolver.eigenvalues();
      
      // Enforce the lower bound
      double abs_lower_bound = eigen_values.maxCoeff()*relative_lower_bound_;
      bool reconstruct_covar = false;
      for (int ii=0; ii<eigen_values.size(); ii++)
      {
        if (eigen_values[ii] < abs_lower_bound)
        {
          eigen_values[ii] = abs_lower_bound;
          reconstruct_covar = true;
        }
      }
      
      // Reconstruct the covariance matrix with the bounded eigenvalues 
      // (but only if the eigenvalues have changed due to the lower bound)
      if (reconstruct_covar)
      {
        MatrixXd eigen_vectors  = eigensolver.eigenvectors();
        covar_new = eigen_vectors*eigen_values.asDiagonal()*eigen_vectors.inverse();
      }
    }
  }
  
  
/*
% Compute absolute lower bound from relative bound and maximum eigenvalue
if (lower_bound_relative~=NO_BOUND)  
  if (lower_bound_relative<0 || lower_bound_relative>1)
    warning('When using a relative lower bound, 0<=bound<=1 must hold, but it is %f. Not setting any lower bounds.',relative_lower_bound); %#ok<WNTAG>
    lower_bound_absolute = NO_BOUND;
  else
    lower_bound_absolute = max([lower_bound_absolute lower_bound_relative*max(eigval)]);
  end
end

% Check for lower bound
if (lower_bound_absolute~=NO_BOUND)
  too_small = eigval<lower_bound_absolute;
  eigval(too_small) = lower_bound_absolute;
end

% Reconstruct covariance matrix from bounded eigenvalues
eigval = diag(eigval);
covar_scaled_bounded = (eigvec*eigval)/eigvec;
  */
  
  distribution_new.set_covar(covar_new);

  
}
lbfgsfloatval_t sparseAECost(
    void* netParam,
    const lbfgsfloatval_t *ptheta,
    lbfgsfloatval_t *grad,
    const int n,
    const lbfgsfloatval_t step)
{
    instanceSP* pStruct = (instanceSP*)(netParam);
    int hiddenSize = pStruct->hiddenSize;
    int visibleSize = pStruct->visibleSize;
    double lambda = pStruct->lambda;
    double beta = pStruct->beta;
    double sp = pStruct->sparsityParam;
    MatrixXd& data = pStruct->data;
    double cost = 0;

    MatrixXd w1(hiddenSize, visibleSize);
    MatrixXd w2(visibleSize, hiddenSize);
    VectorXd b1(hiddenSize);
    VectorXd b2(visibleSize);

    for (int i=0; i<hiddenSize*visibleSize; i++)
    {
        *(w1.data()+i) = *ptheta;
        ptheta++;
    }
    for (int i=0; i<visibleSize*hiddenSize; i++)
    {
        *(w2.data()+i) = *ptheta;
        ptheta++;
    }
    for (int i=0; i<hiddenSize; i++)
    {
        *(b1.data()+i) = *ptheta;
        ptheta++;
    }
    for (int i=0; i<visibleSize; i++)
    {
        *(b2.data()+i) = *ptheta;
        ptheta++;
    }

    int ndim = data.rows();
    int ndata = data.cols();

    MatrixXd z2 = w1 * data + b1.replicate(1, ndata);
    MatrixXd a2 = sigmoid(z2);
    MatrixXd z3 = w2 * a2 + b2.replicate(1, ndata);
    MatrixXd a3 = sigmoid(z3);

    VectorXd rho = a2.rowwise().sum() / ndata;
    VectorXd sparsityDelta = -sp / rho.array() + (1 - sp) / (1 - rho.array());

    MatrixXd delta3 = (a3 - data).array() * sigmoidGrad(z3).array();
    MatrixXd delta2 = (w2.transpose() * delta3 + beta * sparsityDelta.replicate(1, ndata)).array() 
                      * sigmoidGrad(z2).array();

    MatrixXd w1Grad = delta2 * data.transpose() / ndata + lambda * w1;
    VectorXd b1Grad = delta2.rowwise().sum() / ndata;
    MatrixXd w2Grad = delta3 * a2.transpose() / ndata + lambda * w2;
    VectorXd b2Grad = delta3.rowwise().sum() / ndata;

    cost = (0.5 * (a3 - data).array().pow(2)).matrix().sum() / ndata
            + 0.5 * lambda * ((w1.array().pow(2)).matrix().sum() 
            + (w2.array().pow(2)).matrix().sum())
            + beta * (sp * (sp / rho.array()).log() 
            + (1 - sp) * ((1 - sp) / (1 - rho.array())).log() ).matrix().sum();

    double* pgrad = grad;
    for (int i=0; i<hiddenSize*visibleSize; i++)
    {
        *pgrad = *(w1Grad.data()+i);
        pgrad++;
        
    }
    for (int i=0; i<visibleSize*hiddenSize; i++)
    {
        *pgrad = *(w2Grad.data()+i);
        pgrad++;
    }
    for (int i=0; i<hiddenSize; i++)
    {
        *pgrad = *(b1Grad.data()+i);
        pgrad++;
    }
    for (int i=0; i<visibleSize; i++)
    {
        *pgrad = *(b2Grad.data()+i);
        pgrad++;
    }

    return cost;
}