void MarkovExpectation::compute(FitContext *fc, const char *what, const char *how)
{
if (fc) {
for (auto c1 : components) {
c1->compute(fc, what, how);
}
}
omxRecompute(initial, fc);
if (initialV != omxGetMatrixVersion(initial)) {
omxCopyMatrix(scaledInitial, initial);
EigenVectorAdaptor Ei(scaledInitial);
if (scale == SCALE_SOFTMAX) Ei.derived() = Ei.array().exp();
if (scale != SCALE_NONE) {
Ei /= Ei.sum();
}
if (verbose >= 2) mxPrintMat("initial", Ei);
initialV = omxGetMatrixVersion(initial);
}
if (transition) {
omxRecompute(transition, fc);
if (transitionV != omxGetMatrixVersion(transition)) {
omxCopyMatrix(scaledTransition, transition);
EigenArrayAdaptor Et(scaledTransition);
if (scale == SCALE_SOFTMAX) Et.derived() = Et.array().exp();
if (scale != SCALE_NONE) {
Eigen::ArrayXd v = Et.colwise().sum();
Et.rowwise() /= v.transpose();
}
if (verbose >= 2) mxPrintMat("transition", Et);
transitionV = omxGetMatrixVersion(transition);
}
}
}
void arrayops() {
double nx = 10;
double ny = 10;
double nt = 100;
double c = 1;
double dx = 2/(nx-1);
double dy = 2/(ny-1);
double sigma = .2;
double dt = sigma*dx;
Eigen::ArrayXd x; // Array containing our x values
x.setLinSpaced(nx,0,2);
Eigen::ArrayXd y; // Array containing our x values
y.setLinSpaced(ny,0,2);
Eigen::ArrayXXd u;
u.setOnes(ny,nx);
Eigen::ArrayXXd un;
un.setOnes(ny,nx);
u.block(.5/dy,.5/dx,(1/dy+1)-.5/dy,(1/dx+1)-.5/dx) = Eigen::ArrayXXd::Constant((1/dy+1)-.5/dy,(1/dx+1)-.5/dx,2);
int count;
for(int i=0;i<nt;i++) {
un << u;
u.block(1,1,ny-1,nx-1) = un.block(1,1,ny-1,nx-1)-(c*dt/dx*(un.block(1,1,ny-1,nx-1)-un.block(0,1,ny-1,nx-1)))-(c*dt/dy*(un.block(1,1,ny-1,nx-1)-un.block(1,0,ny-1,nx-1)));
}
std::cout << u << std::endl;
}
Eigen::ArrayXXd mpFlow::math::circularPoints(double const radius, double const distance,
double const offset, bool const invertDirection, Eigen::Ref<Eigen::ArrayXd const> const midpoint) {
Eigen::ArrayXd const phi = arange(offset / radius, offset / radius + 2.0 * M_PI, distance / radius);
Eigen::ArrayXXd result = Eigen::ArrayXXd::Zero(phi.rows(), 2);
result.col(0) = midpoint(0) + radius * phi.cos();
result.col(1) = midpoint(1) + (invertDirection ? -1.0 : 1.0) * radius * phi.sin();
return result;
}
// create initial points distribution
Eigen::ArrayXXd distmesh::utils::createInitialPoints(
Functional const& distanceFunction, double const initialPointDistance,
Functional const& elementSizeFunction, Eigen::Ref<Eigen::ArrayXXd const> const boundingBox,
Eigen::Ref<Eigen::ArrayXXd const> const fixedPoints) {
// extract dimension of mesh
unsigned const dimension = boundingBox.cols();
// initially distribute points evenly in complete bounding box
Eigen::ArrayXi pointsPerDimension(dimension);
for (int dim = 0; dim < dimension; ++dim) {
pointsPerDimension(dim) = ceil((boundingBox(1, dim) - boundingBox(0, dim)) /
(initialPointDistance * (dim == 0 ? 1.0 : sqrt(3.0) / 2.0)));
}
Eigen::ArrayXXd points(pointsPerDimension.prod(), dimension);
for (int point = 0; point < points.rows(); ++point)
for (int dim = 0; dim < dimension; ++dim) {
int const pointIndex = (point / std::max(pointsPerDimension.topRows(dim).prod(), 1)) %
pointsPerDimension(dim);
points(point, dim) = boundingBox(0, dim) + (double)pointIndex * initialPointDistance *
(dim == 0 ? 1.0 : sqrt(3.0) / 2.0);
if (dim > 0) {
points(point, dim - 1) += pointIndex % 2 != 0 ? initialPointDistance / 2.0 : 0.0;
}
}
// reject points outside of region defined by distance function
points = selectMaskedArrayElements<double>(points,
distanceFunction(points) < constants::geometryEvaluationThreshold * initialPointDistance);
// clear duplicate points
Eigen::Array<bool, Eigen::Dynamic, 1> isUniquePoint =
Eigen::Array<bool, Eigen::Dynamic, 1>::Constant(points.rows(), true);
for (int i = 0; i < fixedPoints.rows(); ++i)
for (int j = 0; j < points.rows(); ++j) {
isUniquePoint(j) &= !(fixedPoints.row(i) == points.row(j)).all();
}
points = selectMaskedArrayElements<double>(points, isUniquePoint);
// calculate probability to keep points
Eigen::ArrayXd probability = 1.0 / elementSizeFunction(points).pow(dimension);
probability /= probability.maxCoeff();
// reject points with wrong probability
points = selectMaskedArrayElements<double>(points,
0.5 * (1.0 + Eigen::ArrayXd::Random(points.rows())) < probability);
// combine fixed and variable points to one array
Eigen::ArrayXXd finalPoints(points.rows() + fixedPoints.rows(), dimension);
finalPoints << fixedPoints, points;
return finalPoints;
}
void nonlin2d() {
double nx = 101;
double ny = 101;
double nt = 80;
double c = 1;
double dx = 2/(nx-1);
double dy = 2/(ny-1);
double sigma = .2;
double dt = sigma*dx;
// Make an output folder
std::string dir = "step6/";
Eigen::ArrayXd x; // Array containing our x values
x.setLinSpaced(nx,0,2);
Eigen::ArrayXd y; // Array containing our x values
y.setLinSpaced(ny,0,2);
Eigen::ArrayXXd u;
u.setOnes(ny,nx);
Eigen::ArrayXXd v;
v.setOnes(ny,nx);
Eigen::ArrayXXd un;
un.setOnes(ny,nx);
Eigen::ArrayXXd vn;
vn.setOnes(ny,nx);
u.block(.5/dy, .5/dx, 1/dy+1-.5/dy, 1/dx+1-.5/dx) = Eigen::ArrayXXd::Constant(1/dy+1-.5/dy,1/dx+1-.5/dx,2);
v.block(.5/dy, .5/dx, 1/dy+1-.5/dy, 1/dx+1-.5/dx) = Eigen::ArrayXXd::Constant(1/dy+1-.5/dy,1/dx+1-.5/dx,2);
for(int i=0;i<nt+1;i++) {
un << u;
vn << v;
u.block(1,1,ny-1,nx-1) = un.block(1,1,ny-1,nx-1)-(un.block(1,1,ny-1,nx-1)*dt/dx*(un.block(1,1,ny-1,nx-1)-un.block(0,1,ny-1,nx-1)))-(vn.block(1,1,ny-1,nx-1)*dt/dy*(un.block(1,1,ny-1,nx-1)-un.block(1,0,ny-1,nx-1)));
v.block(1,1,ny-1,nx-1) = vn.block(1,1,ny-1,nx-1)-(un.block(1,1,ny-1,nx-1)*dt/dx*(vn.block(1,1,ny-1,nx-1)-vn.block(0,1,ny-1,nx-1)))-(vn.block(1,1,ny-1,nx-1)*dt/dy*(vn.block(1,1,ny-1,nx-1)-vn.block(1,0,ny-1,nx-1)));
u.block(0,0,1,nx-1) = Eigen::ArrayXXd::Constant(1,nx-1,1);
u.block(ny-1,0,1,nx-1) = Eigen::ArrayXXd::Constant(1,nx-1,1);
u.block(0,0,ny-1,1) = Eigen::ArrayXXd::Constant(ny-1,1,1);
u.block(0,nx-1,ny-1,1) = Eigen::ArrayXXd::Constant(ny-1,1,1);
v.block(0,0,1,nx-1) = Eigen::ArrayXXd::Constant(1,nx-1,1);
v.block(ny-1,0,1,nx-1) = Eigen::ArrayXXd::Constant(1,nx-1,1);
v.block(0,0,ny-1,1) = Eigen::ArrayXXd::Constant(ny-1,1,1);
v.block(0,nx-1,ny-1,1) = Eigen::ArrayXXd::Constant(ny-1,1,1);
}
ofstream writexdata((dir + "output.dat").c_str(), ios::out | ios::trunc);
writexdata << u << endl;
writexdata.close();
}
PFloat Verify::mult_weights(Eigen::ArrayXd w)
{
PFloat ret = PFloat(w[0]);
for(int i = 1; i < w.size(); i++)
{
PFloat tmp = PFloat(w(i));
ret = ret*tmp;
}
return ret;
}
double L2R_Huber_SVC::predict(const std::string fileNameLibSVMFormat,
Eigen::VectorXd &w) {
Eigen::SparseMatrix<double, 1> vX;
Eigen::ArrayXd vy;
const char *fn = fileNameLibSVMFormat.c_str();
read_LibSVMdata1(fn, vX, vy);
// loadLib(fileNameLibSVMFormat, vX, vy);
int vl = vX.rows();
int vn = vX.cols();
int success = 0;
if (vn != w.size()) {
w.conservativeResize(vn);
}
Eigen::ArrayXd prob = vy * (vX * w).array();
for (int i = 0; i < vl; ++i) {
if (prob.coeffRef(i) >= 0.0)
++success;
}
return (double)success / vl;
}
/**
* Compute average temperature in all regions.
*
* \param[in] state Dynamic reservoir state.
*/
void
averageTemperature(const BlackoilState& state)
{
T_avg_.setZero();
const std::vector<double>& T = state.temperature();
for (std::vector<double>::size_type
i = 0, n = T.size(); i < n; ++i)
{
T_avg_(rmap_.region(i)) += T[i];
}
T_avg_ /= ncells_;
}
/**
* Compute average hydrocarbon pressure in all regions.
*
* \param[in] state Dynamic reservoir state.
*/
void
averagePressure(const BlackoilState& state)
{
p_avg_.setZero();
const std::vector<double>& p = state.pressure();
for (std::vector<double>::size_type
i = 0, n = p.size(); i < n; ++i)
{
p_avg_(rmap_.region(i)) += p[i];
}
p_avg_ /= ncells_;
}
GLMMBelief::GLMMBelief(const std::vector<int>& items,
const Eigen::MatrixXd& X,
const Eigen::MatrixXd& Zt,
const Eigen::SparseMatrix<double>& Lambdat,
const Eigen::VectorXi& Lind,
const Eigen::ArrayXd& response,
const Eigen::ArrayXd& weights):
ContinuousBeliefBase(items),
numObservations_(response.size()), numFixed_(X.cols()),
X_(X), Zt_(Zt), Lambdat_(Lambdat), Lind_(Lind),
response_(response), weights_(weights)
{
initializeParameterDependents();
}
void laplace2d() {
double nx = 31;
double ny = 31;
double c = 1;
double dx = 2/(nx-1);
double dy = 2/(ny-1);
double linorm_target = .01;
double linorm = 1;
// Make an output folder
std::string dir = "step9/";
Eigen::ArrayXd x; // Array containing our x values
x.setLinSpaced(nx,0,2); // Array is linearly spaced values from 0 to 2
Eigen::ArrayXd y; // Array containing our x values
y.setLinSpaced(ny,0,1);
// Arrays containing our calculated values
Eigen::ArrayXXd p;
Eigen::ArrayXXd pn;
// Initial Conditions
p.setZero(ny,nx);
pn.setZero(ny,nx);
//Set Boundary Conditions
p.block(0,0,ny,1) = Eigen::ArrayXXd::Constant(ny,1,0);
p.block(0,nx-1,ny,1) = y.block(0,0,ny,1);
p.block(0,0,1,nx) = p.block(1,0,1,nx);
p.block(ny-1,0,1,nx) = p.block(ny-2,0,1,nx);
while(linorm>linorm_target)
{
pn << p;
p.block(1,1,ny-2,nx-2) = (pow(dy,2)*(pn.block(2,1,ny-2,nx-2)+pn.block(0,1,ny-2,nx-2))+pow(dx,2)*(pn.block(1,2,ny-2,nx-2)+pn.block(1,0,ny-2,nx-2)))/(2*(pow(dx,2)+pow(dy,2)));
p(0,0) = (pow(dy,2)*(pn(1,0)+pn(ny-1,0))+pow(dx,2)*(pn(0,1)+pn(0,nx-1)))/(2*(pow(dx,2)+pow(dy,2)));
p(ny-1,nx-1) = (pow(dy,2)*(pn(0,nx-1)+pn(ny-2,nx-1))+pow(dx,2)*(pn(ny-1,0)+pn(ny-1,nx-2)))/(2*(pow(dx,2)+pow(dy,2)));
//Set Boundary Conditions
p.block(0,0,ny,1) = Eigen::ArrayXXd::Constant(ny,1,0);
p.block(0,nx-1,ny,1) = y.block(0,0,ny,1);
p.block(0,0,1,nx) = p.block(1,0,1,nx);
p.block(ny-1,0,1,nx) = p.block(ny-2,0,1,nx);
linorm = (p.cwiseAbs()-pn.cwiseAbs()).sum()/(pn.cwiseAbs()).sum();
}
ofstream writexdata((dir + "output.dat").c_str(), ios::out | ios::trunc);
writexdata << p << endl;
writexdata.close();
}
void channelns() {
double nx = 41;
double ny = 41;
double nt = 10;
double nit = 50;
double c = 1;
double dx = 2/(nx-1);
double dy = 2/(ny-1);
double rho = 1;
double nu =.1;
double F = 1;
double dt = .01;
std::string dir = "step11/";
Eigen::ArrayXd x; // Array containing our x values
x.setLinSpaced(nx,0,2); // Array is linearly spaced values from 0 to 2
Eigen::ArrayXd y; // Array containing our x values
y.setLinSpaced(ny,0,2);
Eigen::ArrayXXd X;
Eigen::ArrayXXd Y;
X = x.transpose().replicate(nx,1); // Need to transpose this because the type we declared, ArrayXd, is row-major (and we need column-major for a proper meshgrid)
Y = y.replicate(1,ny);
Eigen::ArrayXXd u;
u.setZero(ny,nx);
Eigen::ArrayXXd un;
un.setZero(ny,nx);
Eigen::ArrayXXd v;
v.setZero(ny,nx);
Eigen::ArrayXXd vn;
vn.setZero(ny,nx);
Eigen::ArrayXXd b;
b.setZero(ny,nx);
Eigen::ArrayXXd p;
p.setZero(ny,nx);
Eigen::ArrayXXd pn;
pn.setZero(ny,nx);
}
void RGRRTNode::FindReachableSet(){
//Need to discretize control space, and fill in reachableStates and reachableControls arrays
Eigen::ArrayXd contactPointLocations = Eigen::ArrayXd::LinSpaced(DISC_S,0,2*LO);
Eigen::ArrayXd normalForceMagnitude = Eigen::ArrayXd::LinSpaced(DISC_FN,MIN_FN,MAX_FN);
Eigen::ArrayXd tangentForceMagnitude = Eigen::ArrayXd::LinSpaced(DISC_FT,-1.0*MU,MU);
Eigen::Matrix<double, CONTROL_SPACE_DIM,1> controlArray;
int counter = 0;
for(int i = 0; i < contactPointLocations.rows(); i++) {
for(int j = 0; j < normalForceMagnitude.rows(); j++) {
for(int k = 0; k < tangentForceMagnitude.rows(); k++) {
controlArray << contactPointLocations(i,0), \
normalForceMagnitude(j,0), \
tangentForceMagnitude(k,0)*normalForceMagnitude(j,0);
reachableStates[counter] = spawn(nodeState,controlArray);
reachableControls[counter] = controlArray;
counter++;
}
}
}
}
void omxComputeNumericDeriv::computeImpl(FitContext *fc)
{
if (fc->fitUnits == FIT_UNITS_SQUARED_RESIDUAL ||
fc->fitUnits == FIT_UNITS_SQUARED_RESIDUAL_CHISQ) { // refactor TODO
numParams = 0;
if (verbose >= 1) mxLog("%s: derivatives %s units are meaningless",
name, fitUnitsToName(fc->fitUnits));
return; //Possible TODO: calculate Hessian anyway?
}
int newWanted = fc->wanted | FF_COMPUTE_GRADIENT;
if (wantHessian) newWanted |= FF_COMPUTE_HESSIAN;
int nf = fc->calcNumFree();
if (numParams != 0 && numParams != nf) {
mxThrow("%s: number of parameters changed from %d to %d",
name, numParams, nf);
}
numParams = nf;
if (numParams <= 0) { complainNoFreeParam(); return; }
optima.resize(numParams);
fc->copyEstToOptimizer(optima);
paramMap.resize(numParams);
for (int px=0,ex=0; px < numParams; ++ex) {
if (fc->profiledOut[ex]) continue;
paramMap[px++] = ex;
}
omxAlgebraPreeval(fitMat, fc);
fc->createChildren(fitMat); // allow FIML rowwiseParallel even when parallel=false
fc->state->countNonlinearConstraints(fc->state->numEqC, fc->state->numIneqC, false);
int c_n = fc->state->numEqC + fc->state->numIneqC;
fc->constraintFunVals.resize(c_n);
fc->constraintJacobian.resize(c_n, numParams);
if(c_n){
omxCalcFinalConstraintJacobian(fc, numParams);
}
// TODO: Allow more than one hessian value for calculation
int numChildren = 1;
if (parallel && !fc->openmpUser && fc->childList.size()) numChildren = fc->childList.size();
if (!fc->haveReferenceFit(fitMat)) return;
minimum = fc->fit;
hessWorkVector = new hess_struct[numChildren];
if (numChildren == 1) {
omxPopulateHessianWork(hessWorkVector, fc);
} else {
for(int i = 0; i < numChildren; i++) {
omxPopulateHessianWork(hessWorkVector + i, fc->childList[i]);
}
}
if(verbose >= 1) mxLog("Numerical Hessian approximation (%d children, ref fit %.2f)",
numChildren, minimum);
hessian = NULL;
if (wantHessian) {
hessian = fc->getDenseHessUninitialized();
Eigen::Map< Eigen::MatrixXd > eH(hessian, numParams, numParams);
eH.setConstant(NA_REAL);
if (knownHessian) {
int khSize = int(khMap.size());
Eigen::Map< Eigen::MatrixXd > kh(knownHessian, khSize, khMap.size());
for (int rx=0; rx < khSize; ++rx) {
for (int cx=0; cx < khSize; ++cx) {
if (khMap[rx] < 0 || khMap[cx] < 0) continue;
eH(khMap[rx], khMap[cx]) = kh(rx, cx);
}
}
}
}
if (detail) {
recordDetail = false; // already done it once
} else {
Rf_protect(detail = Rf_allocVector(VECSXP, 4));
SET_VECTOR_ELT(detail, 0, Rf_allocVector(LGLSXP, numParams));
for (int gx=0; gx < 3; ++gx) {
SET_VECTOR_ELT(detail, 1+gx, Rf_allocVector(REALSXP, numParams));
}
SEXP detailCols;
Rf_protect(detailCols = Rf_allocVector(STRSXP, 4));
Rf_setAttrib(detail, R_NamesSymbol, detailCols);
SET_STRING_ELT(detailCols, 0, Rf_mkChar("symmetric"));
SET_STRING_ELT(detailCols, 1, Rf_mkChar("forward"));
SET_STRING_ELT(detailCols, 2, Rf_mkChar("central"));
SET_STRING_ELT(detailCols, 3, Rf_mkChar("backward"));
SEXP detailRowNames;
Rf_protect(detailRowNames = Rf_allocVector(STRSXP, numParams));
Rf_setAttrib(detail, R_RowNamesSymbol, detailRowNames);
for (int nx=0; nx < int(numParams); ++nx) {
SET_STRING_ELT(detailRowNames, nx, Rf_mkChar(fc->varGroup->vars[nx]->name));
}
markAsDataFrame(detail);
}
gforward = REAL(VECTOR_ELT(detail, 1));
gcentral = REAL(VECTOR_ELT(detail, 2));
gbackward = REAL(VECTOR_ELT(detail, 3));
Eigen::Map< Eigen::ArrayXd > Gf(gforward, numParams);
Eigen::Map< Eigen::ArrayXd > Gc(gcentral, numParams);
Eigen::Map< Eigen::ArrayXd > Gb(gbackward, numParams);
Gf.setConstant(NA_REAL);
Gc.setConstant(NA_REAL);
Gb.setConstant(NA_REAL);
calcHessianEntry che(this);
CovEntrywiseParallel(numChildren, che);
for(int i = 0; i < numChildren; i++) {
struct hess_struct *hw = hessWorkVector + i;
totalProbeCount += hw->probeCount;
}
delete [] hessWorkVector;
if (isErrorRaised()) return;
Eigen::Map< Eigen::ArrayXi > Gsymmetric(LOGICAL(VECTOR_ELT(detail, 0)), numParams);
double gradNorm = 0.0;
double feasibilityTolerance = Global->feasibilityTolerance;
for (int px=0; px < numParams; ++px) {
// factor out simliar code in ComputeNR
omxFreeVar &fv = *fc->varGroup->vars[ paramMap[px] ];
if ((fabs(optima[px] - fv.lbound) < feasibilityTolerance && Gc[px] > 0) ||
(fabs(optima[px] - fv.ubound) < feasibilityTolerance && Gc[px] < 0)) {
Gsymmetric[px] = false;
continue;
}
gradNorm += Gc[px] * Gc[px];
double relsym = 2 * fabs(Gf[px] + Gb[px]) / (Gb[px] - Gf[px]);
Gsymmetric[px] = (Gf[px] < 0 && 0 < Gb[px] && relsym < 1.5);
if (checkGradient && verbose >= 2 && !Gsymmetric[px]) {
mxLog("%s: param[%d] %d %f", name, px, Gsymmetric[px], relsym);
}
}
fc->grad.resize(fc->numParam);
fc->grad.setZero();
fc->copyGradFromOptimizer(Gc);
if(c_n){
fc->inequality.resize(fc->state->numIneqC);
fc->analyticIneqJacTmp.resize(fc->state->numIneqC, numParams);
fc->myineqFun(true, verbose, omxConstraint::LESS_THAN, false);
}
gradNorm = sqrt(gradNorm);
double gradThresh = Global->getGradientThreshold(minimum);
//The gradient will generally not be near zero at a local minimum if there are equality constraints
//or active inequality constraints:
if ( checkGradient && gradNorm > gradThresh && !(fc->state->numEqC || fc->inequality.array().sum()) ) {
if (verbose >= 1) {
mxLog("Some gradient entries are too large, norm %f", gradNorm);
}
if (fc->getInform() < INFORM_NOT_AT_OPTIMUM) fc->setInform(INFORM_NOT_AT_OPTIMUM);
}
fc->setEstFromOptimizer(optima);
// auxillary information like per-row likelihoods need a refresh
ComputeFit(name, fitMat, FF_COMPUTE_FIT, fc);
fc->wanted = newWanted;
}
//[[Rcpp::export]]
List SPMBgraphsqrt(Eigen::Map<Eigen::MatrixXd> data, NumericVector &lambda, int nlambda, int d, NumericVector &x, IntegerVector &col_cnz, IntegerVector &row_idx)
{
Eigen::ArrayXd Xb, r, grad, w1, Y, XX, gr;
Eigen::ArrayXXd X;
Eigen::MatrixXd tmp_icov;
tmp_icov.resize(d, d);
tmp_icov.setZero();
std::vector<Eigen::MatrixXd > tmp_icov_p;
tmp_icov_p.clear();
for(int i = 0; i < nlambda; i++)
tmp_icov_p.push_back(tmp_icov);
int n = data.rows();
X = data;
XX.resize(d);
for (int j = 0; j < d; j++)
XX[j] = (X.col(j)*X.col(j)).sum()/n;
double prec = 1e-4;
int max_iter = 1000;
int num_relaxation_round = 3;
int cnz = 0;
for(int m=0;m<d;m++)
{
Xb.resize(n);
Xb.setZero();
grad.resize(d);
grad.setZero();
gr.resize(d);
gr.setZero();
w1.resize(d);
w1.setZero();
r.resize(n);
r.setZero();
Y = X.col(m);
Eigen::ArrayXd Xb_master(n);
Eigen::ArrayXd w1_master(n);
std::vector<int> actset_indcat(d, 0);
std::vector<int> actset_indcat_master(d, 0);
std::vector<int> actset_idx;
std::vector<double> old_coef(d, 0);
std::vector<double> grad(d, 0);
std::vector<double> grad_master(d, 0);
double a = 0, g = 0, L = 0, sum_r = 0, sum_r2 = 0;
double tmp_change = 0, local_change = 0;
r = Y - Xb;
sum_r = r.sum();
sum_r2 = r.matrix().dot(r.matrix());
L = sqrt(sum_r2 / n);
double dev_thr = fabs(L) * prec;
//cout<<dev_thr<<endl;
for(int i = 0; i < d; i++)
{
grad[i] = (r * X.col(i)).sum() / (n*L);
}
for(int i = 0; i < d; i++) gr[i] = abs(grad[i]);
w1_master = w1;
Xb_master = Xb;
for (int i = 0; i < d; i++) grad_master[i] = gr[i];
std::vector<double> stage_lambdas(d, 0);
for(int i=0;i<nlambda;i++)
{
double ilambda = lambda[i];
w1 = w1_master;
Xb = Xb_master;
for (int j = 0; j < d; j++)
{
gr[j] = grad_master[j];
actset_indcat[j] = actset_indcat_master[j];
}
// init the active set
double threshold;
if (i > 0)
threshold = 2 * lambda[i] - lambda[i - 1];
else
threshold = 2 * lambda[i];
for (int j = 0; j < m; ++j)
{
stage_lambdas[j] = lambda[i];
if (gr[j] > threshold) actset_indcat[j] = 1;
}
for (int j = m+1; j < d; ++j)
{
stage_lambdas[j] = lambda[i];
if (gr[j] > threshold) actset_indcat[j] = 1;
}
stage_lambdas[m] = lambda[i];
r = Y - Xb;
sum_r = r.sum();
sum_r2 = r.matrix().dot(r.matrix());
L = sqrt(sum_r2 / n);
// loop level 0: multistage convex relaxation
int loopcnt_level_0 = 0;
int idx;
double old_w1, updated_coord;
while (loopcnt_level_0 < num_relaxation_round)
{
loopcnt_level_0++;
// loop level 1: active set update
int loopcnt_level_1 = 0;
bool terminate_loop_level_1 = true;
while (loopcnt_level_1 < max_iter)
{
loopcnt_level_1++;
terminate_loop_level_1 = true;
for (int j = 0; j < d; j++) old_coef[j] = w1[j];
// initialize actset_idx
actset_idx.clear();
for (int j = 0; j < m; j++)
if (actset_indcat[j])
{
g = 0.0;
a = 0.0;
double tmp;
sum_r2 = r.matrix().dot(r.matrix());
L = sqrt(sum_r2 / n);
Eigen::ArrayXd wXX = (1 - r*r/sum_r2) * X.col(j) * X.col(j);
g = (wXX * w1[j] + r * X.col(j)).sum()/(n*L);
a = wXX.sum()/(n*L);
tmp = w1[j];
w1[j] = thresholdl1(g, stage_lambdas[j]) / a;
tmp = w1[j] - tmp;
// Xb += delta*X[idx*n]
Xb = Xb + tmp * X.col(j);
sum_r = 0.0;
sum_r2 = 0.0;
// r -= delta*X
r = r - tmp * X.col(j);
sum_r = r.sum();
sum_r2 = r.matrix().dot(r.matrix());
L = sqrt(sum_r2 / n);
updated_coord = w1[j];
if (fabs(updated_coord) > 0) actset_idx.push_back(j);
}
for (int j = m+1; j < d; j++)
if (actset_indcat[j])
{
g = 0.0;
a = 0.0;
double tmp;
sum_r2 = r.matrix().dot(r.matrix());
L = sqrt(sum_r2 / n);
Eigen::ArrayXd wXX = (1 - r*r/sum_r2) * X.col(j) * X.col(j);
g = (wXX * w1[j] + r * X.col(j)).sum()/(n*L);
a = wXX.sum()/(n*L);
tmp = w1[j];
w1[j] = thresholdl1(g, stage_lambdas[j]) / a;
tmp = w1[j] - tmp;
// Xb += delta*X[idx*n]
Xb = Xb + tmp * X.col(j);
sum_r = 0.0;
sum_r2 = 0.0;
// r -= delta*X
r = r - tmp * X.col(j);
sum_r = r.sum();
sum_r2 = r.matrix().dot(r.matrix());
L = sqrt(sum_r2 / n);
updated_coord = w1[j];
if (fabs(updated_coord) > 0) actset_idx.push_back(j);
}
// loop level 2: proximal newton on active set
int loopcnt_level_2 = 0;
bool terminate_loop_level_2 = true;
while (loopcnt_level_2 < max_iter)
{
loopcnt_level_2++;
terminate_loop_level_2 = true;
for (int k = 0; k < actset_idx.size(); k++)
{
idx = actset_idx[k];
old_w1 = w1[idx];
g = 0.0;
a = 0.0;
double tmp;
sum_r2 = r.matrix().dot(r.matrix());
L = sqrt(sum_r2 / n);
Eigen::ArrayXd wXX = (1 - r*r/sum_r2) * X.col(idx) * X.col(idx);
g = (wXX * w1[idx] + r * X.col(idx)).sum()/(n*L);
a = wXX.sum()/(n*L);
tmp = w1[idx];
w1[idx] = thresholdl1(g, stage_lambdas[idx]) / a;
tmp = w1[idx] - tmp;
// Xb += delta*X[idx*n]
Xb = Xb + tmp * X.col(idx);
sum_r = 0.0;
sum_r2 = 0.0;
// r -= delta*X
r = r - tmp * X.col(idx);
sum_r = r.sum();
sum_r2 = r.matrix().dot(r.matrix());
L = sqrt(sum_r2 / n);
updated_coord = w1[idx];
tmp_change = old_w1 - w1[idx];
double a = (X.col(idx) * X.col(idx) * (1 - r * r/(L*L*n))).sum()/(n*L);
local_change = a * tmp_change * tmp_change / (2 * L * n);
if (local_change > dev_thr)
terminate_loop_level_2 = false;
}
if (terminate_loop_level_2)
break;
}
terminate_loop_level_1 = true;
// check stopping criterion 1: fvalue change
for (int k = 0; k < actset_idx.size(); ++k)
{
idx = actset_idx[k];
tmp_change = old_w1 - w1[idx];
double a = (X.col(idx) * X.col(idx) * (1 - r * r/(L*L*n))).sum()/(n*L);
local_change = a * tmp_change * tmp_change / (2 * L * n);
if (local_change > dev_thr)
terminate_loop_level_1 = false;
}
r = Y - Xb;
sum_r = r.sum();
sum_r2 = r.matrix().dot(r.matrix());
L = sqrt(sum_r2 / n);
if (terminate_loop_level_1)
break;
// check stopping criterion 2: active set change
bool new_active_idx = false;
for (int k = 0; k < m; k++)
if (actset_indcat[k] == 0)
{
grad[idx] = (r * X.col(idx)).sum() / (n*L);
//cout<<grad[idx];
gr[k] = fabs(grad[k]);
if (gr[k] > stage_lambdas[k])
{
actset_indcat[k] = 1;
new_active_idx = true;
}
}
for (int k = m+1; k < d; k++)
if (actset_indcat[k] == 0)
{
grad[idx] = (r * X.col(idx)).sum() / (n*L);
//cout<<grad[idx]
gr[k] = fabs(grad[k]);
if (gr[k] > stage_lambdas[k])
{
actset_indcat[k] = 1;
new_active_idx = true;
}
}
if(!new_active_idx)
break;
}
if (loopcnt_level_0 == 1)
{
for (int j = 0; j < d; j++)
{
w1_master[j] = w1[j];
grad_master[j] = gr[j];
actset_indcat_master[j] = actset_indcat[j];
}
for (int j = 0; j < n; j++) Xb_master[j] = Xb[j];
}
}
for(int j=0;j<actset_idx.size();j++)
{
int w_idx = actset_idx[j];
x[cnz] = w1[w_idx];
row_idx[cnz] = i*d+w_idx;
cnz++;
//cout<<cnz<<" ";
}
double tal = 0;
Eigen::MatrixXd temp;
temp.resize(n, 1);
for(int j = 0; j < n; j++)
{
temp(j, 0) = 0;
for(int k = 0; k < d; k++)
temp(j, 0) += X.matrix()(j, k)*w1[k];
temp(j, 0) = Y[j] - temp(j, 0);
}
//temp = Y.matrix() - X.matrix().transpose()*w1.matrix();
for(int j = 0; j < n; j++)
tal += temp(j, 0)*temp(j, 0);
tal = sqrt(tal)/sqrt(n);
tmp_icov = tmp_icov_p[i];
tmp_icov(m, m) = pow(tal, -2);
for(int j = 0; j < m; j++)
tmp_icov(j, m) = -tmp_icov(m, m)*w1[j];
for(int j = m+1; j < d; j++)
tmp_icov(j, m) = -tmp_icov(m, m)*w1[j];
tmp_icov_p[i] = tmp_icov;
}
col_cnz[m+1]=cnz;
}
for(int i = 0; i < nlambda; i++)
tmp_icov_p[i] = (tmp_icov_p[i].transpose()+tmp_icov_p[i])/2;
return List::create(
_["col_cnz"] = col_cnz,
_["row_idx"] = row_idx,
_["x"] = x,
_["icov"] = tmp_icov_p
);
}
