void FFOgkBasis( const MatrixXd& xi, const int& calcM, const int& intercept, VectorXi& warn, const int& h, VectorXi& dIn, int w3 ){ double (*pFo[])(Ref<VectorXd>,int)={&qn,&scaleTau2}; double (*qFo[])(Ref<VectorXd>,int)={&Fmedian,&scaleTau2}; const int p=xi.cols(),n=xi.rows(),h0=(n+1)/2; double b1=0.0,b2=0.0; const double tol=1e-8; int i,j; MatrixXd x=xi; RowVectorXd lamba(p); MatrixXd x2=x; if(intercept){ for(i=0;i<p;i++) lamba(i)=qCalc(x2.col(i),1,qFo[calcM]); x.rowwise()-=lamba; } for(i=0;i<p;i++) lamba(i)=pCalc(x2.col(i),0,pFo[calcM]); for(i=0;i<p;i++) warn(i)=(lamba(i)<tol)?1:0; i=warn.sum(); if(i>0) return; for(i=0;i<p;i++) x.col(i).array()/=lamba(i); VectorXd dvec1=VectorXd::Ones(p); MatrixXd U=dvec1.asDiagonal(); VectorXd sYi(n); VectorXd sYj(n); VectorXd dY(n); for(i=0;i<p;++i){ sYi=x.col(i); for(j=0;j<i;++j){ sYj=x.col(j); dY=sYi+sYj; b1=pCalc(dY,0,pFo[calcM]); b1*=b1; dY=sYi-sYj; b2=pCalc(dY,0,pFo[calcM]); b2*=b2; U(i,j)=0.25*(b1-b2); U(j,i)=U(i,j); } } JacobiSVD<MatrixXd> svd(U,ComputeThinV); x2=x*svd.matrixV(); for(i=0;i<p;i++) lamba(i)=pCalc(x2.col(i),0,pFo[calcM]); for(i=0;i<p;i++) warn(i)=(lamba(i)<tol)?1:0; i=warn.sum(); if(i>0) return; for(i=0;i<p;i++) x2.col(i).array()/=lamba(i); dY=x2.array().abs2().rowwise().sum(); dIn.setLinSpaced(n,0,n-1); std::nth_element(dIn.data(),dIn.data()+h,dIn.data()+dIn.size(),IdLess(dY.data())); cov_CStep(dIn,x,h,h,w3); return; }
void cov_CStep( VectorXi& dIn, MatrixXd& x, const int h, const int h0, int w3 ){ const int n=x.rows(),p=x.cols(); double w1,w0; int w2=1,i; MatrixXd xSub(h,p); for(i=0;i<h0;i++) xSub.row(i)=x.row(dIn(i)); RowVectorXd xSub_mean(p); xSub_mean=xSub.topRows(h0).colwise().mean(); xSub.topRows(h0).rowwise()-=xSub_mean; x.rowwise()-=xSub_mean; MatrixXd Sig(p,p); //Sig=xSub.topRows(h0).adjoint()*xSub.topRows(h0); Sig.setZero().selfadjointView<Lower>().rankUpdate(xSub.topRows(h0).transpose()); Sig.array()/=(double)(h0-1); LDLT<MatrixXd> chol=Sig.ldlt(); MatrixXd b=MatrixXd::Identity(p,p); chol.solveInPlace(b); w1=chol.vectorD().array().minCoeff(); VectorXd dP(n); if(w1>1e-6){ w1=std::numeric_limits<double>::max(); dP=((x*b).cwiseProduct(x)).rowwise().sum(); } else { w2=0; w3=0; w1=chol.vectorD().array().log().sum()*2.00; } while(w2){ dIn.setLinSpaced(n,0,n-1); std::nth_element(dIn.data(),dIn.data()+h,dIn.data()+dIn.size(),IdLess(dP.data())); for(i=0;i<h;i++) xSub.row(i)=x.row(dIn(i)); xSub_mean=xSub.colwise().mean(); xSub.rowwise()-=xSub_mean; x.rowwise()-=xSub_mean; //Sig=xSub.adjoint()*xSub; Sig.setZero().selfadjointView<Lower>().rankUpdate(xSub.transpose()); Sig.array()/=(double)(h-1); chol=Sig.ldlt(); b=MatrixXd::Identity(p,p); chol.solveInPlace(b); if(chol.vectorD().array().minCoeff()>1e-6){ w0=w1; w1=chol.vectorD().array().log().sum()*2.00; dP=((x*b).cwiseProduct(x)).rowwise().sum(); (w0-w1<1e-3)?(w2=0):(w2=1); } else { w2=0; w3=0; } } }