// ---------------------------------------------------------------------------
// Iteratively compute the value of the likelihood function
// at a range of frequencies around the peak likelihood.
// Return the maximum value when the range of likelihood
// values computed is less than the specified resolution.
double
iterative_estimate( const vector<double> & amps,
const vector<double> & freqs,
double fmin, double fmax,
double resolution )
{
//static const double NinetyNinePercent = .99; // stop when within 1% of maximum
// never consider DC (0 Hz) to be a valid fundamental
fmin = std::max( 1., fmin );
// when the frequency range is small, few samples are
// needed, but initially make sure to sample at least
// every 20 Hz.
// Scratch that, 20 Hz isn't fine enough, could miss a
// peak that way, try 2 Hz. There might be some room to
// adjust this parameter to trade off speed for robustness.
int Nsamps = std::max( 8, (int)std::ceil((fmax-fmin)*0.5) );
vector<double> eval_freqs, Q;
double peak_freq = fmin, peak_Q = 1;
// invariant:
// the likelihood function for the estimate of the fundamental
// frequency is maximized at some frequency between
// fmin and fmax (stop when that range is smaller
// than the resolution)
do
{
// determine the frequencies at which to evaluate
// the likelihood function
eval_freqs.resize( Nsamps );
compute_eval_freqs( fmin, fmax, eval_freqs );
// evaluate the likelihood function at those
// frequencies:
Q.resize( Nsamps );
evaluate_Q( amps, freqs, eval_freqs, Q );
// find the highest frequency at which the likelihood
// function peaks:
vector<double>::const_iterator peak = choose_peak( Q );
int peak_idx = peak - Q.begin();
peak_Q = *peak;
peak_freq = eval_freqs[ peak_idx ];
// update search range:
fmin = eval_freqs[ std::max(peak_idx - 1, 0) ];
fmax = eval_freqs[ std::min(peak_idx + 1, Nsamps - 1) ];
Nsamps = std::max( 8, (int)std::ceil((fmax-fmin)*0.05) );
} while ( (fmax - fmin) > resolution );
//while( (*std::min_element( Q.begin(), Q.end() ) / peak_Q) < NinetyNinePercent );
return peak_freq;
}
/* main routine to do graduated assignment
*
*/
void gap(long *y, long double *C, int A, int I, long double delta){
long double beta_0 = 0.5;
long double beta = beta_0;
long double beta_f = 10;
long double beta_r = 1.075;
int I_0 = 4;
int I_1 = 30;
//long double epsilon = 0.5;
long double epsilon_0 = 0.5;
long double epsilon_1 = 0.05;
long double err0 = GAP_INFINITY;
long double err1 = GAP_INFINITY;
int i0,i1;
long double **Mt = (long double **)malloc(sizeof(long double*)*(A+1));
long double **Mt0 = (long double **)malloc(sizeof(long double*)*(A+1));
long double **Mt1 = (long double **)malloc(sizeof(long double*)*(A+1));
long double **Q = (long double **)malloc(sizeof(long double*)*A);
int i,a;
int maxind;
long double maxval;
/* normalize C first */
//if (delta > 0) normalize_matrix(C,A,I,delta);
/* allocate memory for Mt */
for (i=0;i<A+1;i++){
Mt[i] = (long double *)malloc(sizeof(long double)*(I+1));
Mt0[i] = (long double *)malloc(sizeof(long double)*(I+1));
Mt1[i] = (long double *)malloc(sizeof(long double)*(I+1));
}
/* allocate memory for Q */
for (i=0;i<A;i++){
Q[i] = (long double *)malloc(sizeof(long double)*I);
}
srand48(10); /* seed for random generator */
/* initialize random matching matrix */
for (a=0;a<A+1;a++){
for (i=0;i<I+1;i++){
Mt[a][i] = 1 + 0.25*drand48();
}
}
//print_matrix(Mt,A,I);
while (beta < beta_f){
i0 = 0;
while (i0 < I_0 && err0 > epsilon_0){
i0++;
copy_matrix(Mt0, Mt, A+1, I+1);
evaluate_Q(Q, C, Mt, A, I); /* evaluate Q is done without the dummy nodes */
evaluate_Mt(Mt, Q, beta, A, I);/* evaluate Mt is done without the dummy nodes */
i1 = 0;
while (i1 < I_1 && err1 > epsilon_1){
i1++;
copy_matrix(Mt1, Mt, A+1, I+1);
normalize_by_row(Mt, A+1, I+1);
normalize_by_column(Mt, A+1, I+1);
err1 = compute_err(Mt, Mt1, A+1, I+1);
}
err1 = GAP_INFINITY;
err0 = compute_err(Mt, Mt0, A+1, I+1);
}
err0 = GAP_INFINITY;
beta = beta*beta_r;
}
//printf("Mt:\n");
//print_matrix(Mt,A,I);
//printf("\n");
/* try to find the matching vector */
for (a=0;a<A;a++){
maxval = Mt[a][0];
maxind = 0;
for (i=1;i<I;i++){
if (Mt[a][i] > maxval){
maxval = Mt[a][i];
maxind = i;
}
}
y[a] = maxind;
}
//printf("ybar:\n");
//print_matrix(y, A, I);
//printf("\n");
/* free */
for (i=0;i<A;i++){
free(Q[i]);
}
free(Q);
for (i=0;i<A+1;i++){
free(Mt[i]);
free(Mt0[i]);
free(Mt1[i]);
}
free(Mt);
free(Mt0);
free(Mt1);
}
