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);
}
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;
}
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;
}
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();
}
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;
}
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;
}
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;
}
