// ALPHA update
void alpha_update(double *alpha, int i, int j, int k, double *y_aux, double *x_aux, double *rho, double *c, double **y, double lambda, double *alpha_weights, int T, int N, int P){
int ip, jp, kp, l, t;
double c_yx = 0;
double c_xx = 0;
double kappa;
for( ip=0; ip<N; ++ip ){
for( t=P; t<T; ++t ){
kappa = (ip==i)? 1.0 : (-rho[ RHOIDX(ip,i) ] * sqrt(c[ip]/c[i]));
y_aux[ ip*T+t ] += alpha[ ALPIDX(i,j,k,N,P) ] * kappa * y[t-k-1][j];
x_aux[ ip*T+t ] = kappa * y[t-k-1][j];
c_yx += y_aux[ ip*T+t ] * x_aux[ ip*T+t ];
c_xx += x_aux[ ip*T+t ] * x_aux[ ip*T+t ];
}
}
alpha[ ALPIDX(i,j,k,N,P) ] = soft_thresholding(c_yx,c_xx,lambda*alpha_weights[ALPIDX(i,j,k,N,P)]);
//Rprintf("EXACT: %d %d %d -> %f %f : beta_ls %f beta_lasso %f\n",1+i,1+j,1+k,c_yx,c_xx,c_yx/c_xx,alpha[ ALPIDX(i,j,k,N,P) ]);
if( alpha[ ALPIDX(i,j,k,N,P) ] != 0.0 )
{
for( ip=0; ip<N; ++ip ){
for( t=P; t<T; ++t ){
y_aux[ ip*T+t ] -= alpha[ ALPIDX(i,j,k,N,P) ] * x_aux[ ip*T+t ];
}
}
}
}
// ALPHA update init
void alpha_update_init(double *alpha, int i, int j, int k, double **y, double lambda, double *alpha_weights, int T, int N, int P){
int t;
double c_yx = 0;
double c_xx = 0;
for( t=P; t<T; ++t ){
c_yx += y[t][i] * y[t-k-1][j];
c_xx += y[t-k-1][j] * y[t-k-1][j];
}
alpha[ ALPIDX(i,j,k,N,P) ] = soft_thresholding(c_yx,c_xx,lambda*alpha_weights[ALPIDX(i,j,k,N,P)]);
}
// RHO update
void rho_update(double *rho, int i, int j, double *y_aux, double *x_aux, double *alpha, double *c, double **y, double lambda, double *rho_weights, int T, int N, int P){
int t, ip, k;
double c_yx = 0;
double c_xx = 0;
memset(x_aux,0,sizeof(double)*N*T);
for( t=P; t<T; ++t ){
x_aux[ i*T+t ] = y[t][j];
x_aux[ j*T+t ] = y[t][i];
// resid
for( ip=0; ip<N; ++ip ){
for( k=0; k<P; ++k ){
x_aux[ i*T+t ] -= alpha[ ALPIDX(j,ip,k,N,P) ] * y[t-k-1][ip];
x_aux[ j*T+t ] -= alpha[ ALPIDX(i,ip,k,N,P) ] * y[t-k-1][ip];
}
}
x_aux[ i*T+t ] = sqrt(c[j]/c[i]) * x_aux[ i*T+t ];
x_aux[ j*T+t ] = sqrt(c[i]/c[j]) * x_aux[ j*T+t ];
y_aux[ i*T+t ] += rho[ RHOIDX(i,j) ] * x_aux[ i*T+t ];
y_aux[ j*T+t ] += rho[ RHOIDX(i,j) ] * x_aux[ j*T+t ];
c_yx += y_aux[ i*T+t ] * x_aux[ i*T+t ] + y_aux[ j*T+t ] * x_aux[ j*T+t ];
c_xx += x_aux[ i*T+t ] * x_aux[ i*T+t ] + x_aux[ j*T+t ] * x_aux[ j*T+t ];
}
// update alpha
rho[ RHOIDX(i,j) ] = soft_thresholding(c_yx,c_xx,lambda*rho_weights[RHOIDX(i,j)] );
if( rho[ RHOIDX(i,j) ] != 0.0 )
{
for( t=P; t<T; ++t ){
y_aux[ i*T+t ] -= rho[ RHOIDX(i,j) ] * x_aux[ i*T+t ];
y_aux[ j*T+t ] -= rho[ RHOIDX(i,j) ] * x_aux[ j*T+t ];
}
}
}
// (simple) SHOOTING
void nets_shooting(double *alpha, double *rho, double *alpha_weights, double *rho_weights, double *lambda, double *_y, int *_T, int *_N, int *_P, double *c, int *GN, int *CN, int *v, int *m, double *rss, double *npar) {
// variables
int T, N, P;
int granger_network, parcorr_network;
double **y;
double *y_aux, *x_aux;
double *alpha_old;
double *rho_old;
double delta, toll;
int iter, maxiter;
int verbose;
int t, i, j, k, p;
double c_yx, c_xx;
// init
maxiter = *m;
toll = 1e-4;
verbose = *v;
T = *_T;
N = *_N;
P = *_P;
granger_network = *GN;
parcorr_network = *CN;
// allocate memory
y = (double **) R_alloc(T,sizeof(double*));
for( t=0; t<T; t++) {
y[t] = (double *) R_alloc(N,sizeof(double));
for( i=0; i<N; i++ ) {
y[t][i] = _y[ T * i + t ];
}
}
alpha_old = (double *) R_alloc(N*N*P,sizeof(double));
rho_old = (double *) R_alloc(N*(N-1)/2,sizeof(double));
y_aux = (double *) R_alloc(T*N,sizeof(double));
x_aux = (double *) R_alloc(T*N,sizeof(double));
// create residual
memset(y_aux,0,sizeof(double)*N*T);
for( i=0; i<N; ++i ){
for( t=P; t<T; ++t ){
y_aux[i*T+t] = y[t][i];
// ALPHA
for( j=0; j<N; ++j ){
for( p=0; p<P; ++p ){
y_aux[i*T+t] -= alpha[ ALPIDX(i,j,p,N,P) ] * y[t-p-1][j];
}
}
// RHO
for( j=0; j<N; ++j){
if( j!=i ){
y_aux[i*T+t] -= rho[ RHOIDX(i,j) ] * sqrt(c[j]/c[i]) * y[t][j];
for( p=0; p<P; ++p ){
for( k=0; k<N; ++k ){
y_aux[i*T+t] += rho[ RHOIDX(i,j) ] * sqrt(c[j]/c[i]) * alpha[ ALPIDX(j,k,p,N,P) ] * y[t-p-1][k];
}
}
}
}
}
}
// main loop
for( iter=1; iter<=maxiter; ++iter ){
if( verbose ) nets_log(alpha,rho,y_aux,lambda,alpha_weights,rho_weights,T,N,P,granger_network,parcorr_network,iter,delta);
memcpy(alpha_old,alpha,sizeof(double)*(N*N*P));
memcpy(rho_old ,rho ,sizeof(double)*(N*(N-1)/2));
// ALPHA
for( i=0; i<N; ++i){
for( j=0; j<N; ++j){
for( p=0; p<P; ++p ){
// compute: y_aux, x_aux, c_yx, c_xx
c_yx = 0;
c_xx = 0;
for( k=0; k<N; ++k ){
for( t=P; t<T; ++t ){
if( k==i ){
x_aux[ k*T+t ] = y[t-p-1][j];
}
else{
x_aux[ k*T+t ] = -rho[ RHOIDX(i,k) ] * sqrt(c[k]/c[i]) * y[t-p-1][j];
}
y_aux[ k*T+t ] += alpha[ ALPIDX(i,j,p,N,P) ]*x_aux[ k*T+t ];
c_yx += y_aux[ k*T+t ] * x_aux[ k*T+t ];
c_xx += x_aux[ k*T+t ] * x_aux[ k*T+t ];
}
}
// update alpha
alpha[ ALPIDX(i,j,p,N,P) ] = soft_thresholding(c_yx,c_xx,lambda[0]*alpha_weights[ALPIDX(i,j,p,N,P)]);
#ifdef DEBUG
Rprintf("VERY EXACT: %d %d %d -> %f %f : beta_ls %f beta_lasso %f\n",1+i,1+j,1+p,c_yx,c_xx,c_yx/c_xx,alpha[ ALPIDX(i,j,p,N,P) ]);
#endif
if( alpha[ ALPIDX(i,j,p,N,P) ] != 0.0 )
{
for( k=0; k<N; ++k ){
for( t=P; t<T; ++t ){
y_aux[ k*T+t ] -= alpha[ ALPIDX(i,j,p,N,P) ] * x_aux[ k*T+t ];
}
}
}
}
}
}
// RHO
for( i=0; i<N; ++i){
for( j=0; j<i; ++j ){
// compute: y_aux, x_aux, c_yx, c_xx
c_yx = 0;
c_xx = 0;
memset(x_aux,0,sizeof(double)*N*T);
for( t=P; t<T; ++t ){
x_aux[ i*T+t ] = y[t][j];
x_aux[ j*T+t ] = y[t][i];
// resid
for( k=0; k<N; ++k ){
for( p=0; p<P; ++p ){
x_aux[ i*T+t ] -= alpha[ ALPIDX(j,k,p,N,P) ] * y[t-p-1][k];
x_aux[ j*T+t ] -= alpha[ ALPIDX(i,k,p,N,P) ] * y[t-p-1][k];
}
}
x_aux[ i*T+t ] = sqrt(c[j]/c[i]) * x_aux[ i*T+t ];
x_aux[ j*T+t ] = sqrt(c[i]/c[j]) * x_aux[ j*T+t ];
y_aux[ i*T+t ] += rho[ RHOIDX(i,j) ] * x_aux[ i*T+t ];
y_aux[ j*T+t ] += rho[ RHOIDX(i,j) ] * x_aux[ j*T+t ];
c_yx += y_aux[ i*T+t ] * x_aux[ i*T+t ] + y_aux[ j*T+t ] * x_aux[ j*T+t ];
c_xx += x_aux[ i*T+t ] * x_aux[ i*T+t ] + x_aux[ j*T+t ] * x_aux[ j*T+t ];
}
// update alpha
rho[ RHOIDX(i,j) ] = soft_thresholding(c_yx,c_xx,lambda[1]*rho_weights[RHOIDX(i,j)] );
#ifdef DEBUG
Rprintf("%d,%d > c_yx %f c_xx %f lambda %f > rho %f\n",i,j,c_yx,c_xx,lambda[1],rho[RHOIDX(i,j)]);
#endif
// update y_aux (if new coeff is != 0)
if( rho[ RHOIDX(i,j) ] != 0.0 )
{
for( t=P; t<T; ++t ){
y_aux[ i*T+t ] -= rho[ RHOIDX(i,j) ] * x_aux[ i*T+t ];
y_aux[ j*T+t ] -= rho[ RHOIDX(i,j) ] * x_aux[ j*T+t ];
}
}
}
}
// check for convergence
delta = 0;
for( i=0; i<N*N*P ; ++i ){ delta += fabs(alpha[i]-alpha_old[i]); }
for( i=0; i<N*(N-1)/2; ++i ){ delta += fabs(rho[i]-rho_old[i]); }
if( delta<toll ) break;
}
if( verbose ) nets_log(alpha,rho,y_aux,lambda,alpha_weights,rho_weights,T,N,P,granger_network,parcorr_network,0,delta);
// rss and npar
*rss = 0.0;
for( i=0; i<N; i++ ) {
c[i] = 0.0;
for( t=0; t<T; t++) {
*rss += y_aux[ T * i + t ]*y_aux[ T * i + t ];
c[i] += y_aux[ T * i + t ]*y_aux[ T * i + t ];
}
c[i] = 1/c[i];
}
*npar = 0.0;
for( i=0; i<N*N*P; ++i ){
*npar += fabs(alpha[i])>0?1:0;
}
for( i=0; i<N*(N-1)/2; ++i ){
*npar += fabs(rho[i])>0?1:0;
}
}
void line_search(int n, float *x, float *grad, float *direction, sf_gradient gradient, sf_optim opt, float *threshold, int *flag, int cpuid, int type)
/*< line search (Wolfe condition) >*/
{
int i, j;
float m1, m2, m3, fcost, alpha1=0., alpha2=0.;
float *xk;
xk=sf_floatalloc(n);
copy(n, x, xk);
dot_product(n, grad, direction, &m1);
m2=m1*opt->c2;
m1=m1*opt->c1;
for(i=0; i<opt->nls; i++){
opt->ils += 1;
for(j=0; j<n; j++)
x[j] =xk[j] + opt->alpha*direction[j];
/* seislet regularization */
if(seislet){
seislet_lop(true, false, n, n, ss, x);
soft_thresholding(ss, n, pclip);
seislet_lop(false, false, n, n, ss, x);
}
/* model constraints */
if(type==1){
clip(x, n, threshold[0], threshold[1]);
}else if(type==2){
clip(x, n/2, threshold[0], threshold[1]);
clip(x+n/2, n/2, threshold[2], threshold[3]);
}
gradient(x, &fcost, grad);
opt->igrad += 1;
dot_product(n, grad, direction, &m3);
if(cpuid==0){
sf_warning("line search i=%d",i+1);
sf_warning("alpha1=%f alpha2=%f alpha=%f",alpha1, alpha2, opt->alpha);
sf_warning("fcost=%f fk=%f fk+c1*alpha*m1=%f m3=%f c2*m1=%f ",fcost, opt->fk, opt->fk+opt->alpha*m1, m3, m2);
}
//if(counter1==4 && i==0) return;
/* Wolfe condition */
if(fcost <= opt->fk + opt->alpha*m1 && m3 >= m2){
opt->fk=fcost;
*flag=0;
break;
}else if (fcost > opt->fk + opt->alpha*m1){
alpha2=opt->alpha;
opt->alpha=0.5*(alpha1+alpha2);
}else{
alpha1=opt->alpha;
if(alpha2 == 0.)
opt->alpha *= opt->factor;
else
opt->alpha = 0.5*(alpha1+alpha2);
}
if(cpuid==0){
sf_warning("After adjustment, alpha1=%f alpha2=%f alpha=%f",alpha1, alpha2, opt->alpha);
}
} // end of line search
if(i==opt->nls){
if(fcost <= opt->fk)
*flag=1;
else
*flag=2;
}
free(xk);
}
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
{
double *v, *ans, s1, s2;
double *ind;
double *x;
double *u, *tmpV, *tmpU, *tmpW;
double up, mid, low, h_s1, lambda0, lambda;
int n, grpNum;
int i, j, k;
if(nlhs != 1)
mexErrMsgTxt("Error: Number of output parameter does not match.");
if(nrhs !=4)
mexErrMsgTxt("Error: Number of input parameter does not match.");
/*
* Input Parameter handle
*/
v = mxGetPr(prhs[0]);
s1 = mxGetScalar(prhs[1]);
s2 = mxGetScalar(prhs[2]);
ind = mxGetPr(prhs[3]);
n = (int)mxGetNumberOfElements(prhs[0]);
grpNum = (int)mxGetNumberOfElements(prhs[3]) - 1;
/*
* Output Parameter handle
*/
plhs[0] = mxCreateDoubleMatrix(n, 1, mxREAL);
ans = mxGetPr(plhs[0]);
/* Temporary variables. */
lambda = lambda0 = .0;
x = (double *)malloc(n * sizeof(double));
tmpV = (double *)malloc(n * sizeof(double));
tmpU = (double *)malloc(grpNum * sizeof(double));
tmpW = (double *)malloc(grpNum * sizeof(double));
if(l1Norm(v, n) <= s1 + 1e-12 && grpNorm(v, ind, grpNum) <= s2 + 1e-12)
{
for(i = 0; i < n; i++)
ans[i] = v[i];
free(x);
free(tmpV);
free(tmpU);
free(tmpW);
return;
}
lambda0 = .0;
eplb(x, &lambda, &k, v, n, s1, lambda0);
if(grpNorm(x, ind, grpNum) < s2 - 1e-12)
{
for(i = 0; i < n; i++)
ans[i] = x[i];
free(x);
free(tmpV);
free(tmpU);
free(tmpW);
return;
}
epglb(x, v, n, s2, ind, grpNum, tmpW, tmpU);
if(l1Norm(x, n) < s1 - 1e-12)
{
for(i = 0; i < n; i++)
ans[i] = x[i];
free(x);
free(tmpV);
free(tmpU);
free(tmpW);
return;
}
up = 1e15, low = 1e-12;
while(up - low > 1e-7)
{
if(low > 1e6 && up - low < 1e-5)
break;
mid = (up + low) / 2;
soft_thresholding(tmpV, v, n, mid);
if(grpNorm(tmpV, ind, grpNum) < s2 - 1e-15)
up = mid;
else{
epglb(x, tmpV, n, s2, ind, grpNum, tmpW, tmpU);
h_s1 = l1Norm(x, n);
if(h_s1 < s1 + 1e-15)
up = mid;
else
low = mid;
}
}
soft_thresholding(tmpV, v, n, up);
epglb(x, tmpV, n, s2, ind, grpNum, tmpW, tmpU);
for(i = 0; i < n; i++)
ans[i] = x[i];
free(x);
free(tmpV);
free(tmpU);
free(tmpW);
}
static void filter(VagueDenoiserContext *s, AVFrame *in, AVFrame *out)
{
int p, y, x, i, j;
for (p = 0; p < s->nb_planes; p++) {
const int height = s->planeheight[p];
const int width = s->planewidth[p];
const uint8_t *srcp8 = in->data[p];
const uint16_t *srcp16 = (const uint16_t *)in->data[p];
uint8_t *dstp8 = out->data[p];
uint16_t *dstp16 = (uint16_t *)out->data[p];
float *output = s->block;
int h_low_size0 = width;
int v_low_size0 = height;
int nsteps_transform = s->nsteps;
int nsteps_invert = s->nsteps;
const float *input = s->block;
if (!((1 << p) & s->planes)) {
av_image_copy_plane(out->data[p], out->linesize[p], in->data[p], in->linesize[p],
s->planewidth[p], s->planeheight[p]);
continue;
}
if (s->depth <= 8) {
for (y = 0; y < height; y++) {
for (x = 0; x < width; x++)
output[x] = srcp8[x];
srcp8 += in->linesize[p];
output += width;
}
} else {
for (y = 0; y < height; y++) {
for (x = 0; x < width; x++)
output[x] = srcp16[x];
srcp16 += in->linesize[p] / 2;
output += width;
}
}
while (nsteps_transform--) {
int low_size = (h_low_size0 + 1) >> 1;
float *input = s->block;
for (j = 0; j < v_low_size0; j++) {
copy(input, s->in + NPAD, h_low_size0);
transform_step(s->in, s->out, h_low_size0, low_size, s);
copy(s->out + NPAD, input, h_low_size0);
input += width;
}
low_size = (v_low_size0 + 1) >> 1;
input = s->block;
for (j = 0; j < h_low_size0; j++) {
copyv(input, width, s->in + NPAD, v_low_size0);
transform_step(s->in, s->out, v_low_size0, low_size, s);
copyh(s->out + NPAD, input, width, v_low_size0);
input++;
}
h_low_size0 = (h_low_size0 + 1) >> 1;
v_low_size0 = (v_low_size0 + 1) >> 1;
}
if (s->method == 0)
hard_thresholding(s->block, width, height, width, s->threshold, s->percent);
else if (s->method == 1)
soft_thresholding(s->block, width, height, width, s->threshold, s->percent, s->nsteps);
else
qian_thresholding(s->block, width, height, width, s->threshold, s->percent);
s->hlowsize[0] = (width + 1) >> 1;
s->hhighsize[0] = width >> 1;
s->vlowsize[0] = (height + 1) >> 1;
s->vhighsize[0] = height >> 1;
for (i = 1; i < s->nsteps; i++) {
s->hlowsize[i] = (s->hlowsize[i - 1] + 1) >> 1;
s->hhighsize[i] = s->hlowsize[i - 1] >> 1;
s->vlowsize[i] = (s->vlowsize[i - 1] + 1) >> 1;
s->vhighsize[i] = s->vlowsize[i - 1] >> 1;
}
while (nsteps_invert--) {
const int idx = s->vlowsize[nsteps_invert] + s->vhighsize[nsteps_invert];
const int idx2 = s->hlowsize[nsteps_invert] + s->hhighsize[nsteps_invert];
float * idx3 = s->block;
for (i = 0; i < idx2; i++) {
copyv(idx3, width, s->in + NPAD, idx);
invert_step(s->in, s->out, s->tmp, idx, s);
copyh(s->out + NPAD, idx3, width, idx);
idx3++;
}
idx3 = s->block;
for (i = 0; i < idx; i++) {
copy(idx3, s->in + NPAD, idx2);
invert_step(s->in, s->out, s->tmp, idx2, s);
copy(s->out + NPAD, idx3, idx2);
idx3 += width;
}
}
if (s->depth <= 8) {
for (y = 0; y < height; y++) {
for (x = 0; x < width; x++)
dstp8[x] = av_clip_uint8(input[x] + 0.5f);
input += width;
dstp8 += out->linesize[p];
}
} else {
for (y = 0; y < height; y++) {
for (x = 0; x < width; x++)
dstp16[x] = av_clip(input[x] + 0.5f, 0, s->peak);
input += width;
dstp16 += out->linesize[p] / 2;
}
}
}
}
