colorchord/decompose.c
2015-01-06 22:51:39 -05:00

216 lines
5.1 KiB
C

#include "decompose.h"
#include <stdio.h>
#include <math.h>
#include <stdlib.h>
#include <string.h>
#define SQRT2PI 2.506628253
#ifdef TURBO_DECOMPOSE
int DecomposeHistogram( float * histogram, int bins, float * out_means, float * out_amps, float * out_sigmas, int max_dists, double default_sigma, int iterations )
{
//Step 1: Find the actual peaks.
int i;
int peak = 0;
for( i = 0; i < bins; i++ )
{
float offset = 0;
float prev = histogram[(i-1+bins)%bins];
float this = histogram[i];
float next = histogram[(i+1)%bins];
if( prev > this || next > this ) continue;
if( prev == this && next == this ) continue;
//i is at a peak...
float totaldiff = (( this - prev ) + ( this - next ));
float porpdiffP = (this-prev)/totaldiff; //close to 0 = closer to this side... 0.5 = in the middle ... 1.0 away.
float porpdiffN = (this-next)/totaldiff;
if( porpdiffP < porpdiffN )
{
//Closer to prev.
offset = -(0.5 - porpdiffP);
}
else
{
offset = (0.5 - porpdiffN);
}
out_means[peak] = i + offset;
//XXX XXX TODO Examine difference or relationship of "this" and "totaldiff"
out_amps[peak] = this * 4;
//powf( totaldiff, .8) * 10;//powf( totaldiff, .5 )*4; //
out_sigmas[peak] = default_sigma;
peak++;
}
for( i = peak; i < max_dists; i++ )
{
out_means[i] = -1;
out_amps[i] = 0;
out_sigmas[i] = default_sigma;
}
return peak;
}
//Yick: Doesn't match.. I guess it's only for debugging, right?
float CalcHistAt( float pt, int bins, float * out_means, float * out_amps, float * out_sigmas, int cur_dists )
{
int i;
float mark = 0.0;
for( i = 0; i < cur_dists; i++ )
{
float amp = out_amps[i];
float mean = out_means[i];
float var = out_sigmas[i];
float x = mean - pt;
if( x < - bins / 2 ) x += bins;
if( x > bins / 2 ) x -= bins;
float nrm = amp / (var * SQRT2PI ) * expf( - ( x * x ) / ( 2 * var * var ) );
mark += nrm;
}
return mark;
}
#else
float AttemptDecomposition( float * histogram, int bins, float * out_means, float * out_amps, float * out_sigmas, int cur_dists );
float CalcHistAt( float pt, int bins, float * out_means, float * out_amps, float * out_sigmas, int cur_dists );
int DecomposeHistogram( float * histogram, int bins, float * out_means, float * out_amps, float * out_sigmas, int max_dists, double default_sigma, int iterations )
{
//NOTE: The sigma may change depending on all sorts of things, maybe?
int sigs = 0;
int i;
int went_up = 0;
float vhist = histogram[bins-1];
for( i = 0; i <= bins; i++ )
{
float thishist = histogram[i%bins];
if( thishist > vhist )
{
went_up = 1;
}
else if( went_up)
{
went_up = 0;
out_amps[sigs] = thishist / (default_sigma + 1);
out_sigmas[sigs] = default_sigma;
out_means[sigs] = i-0.5;
sigs++;
}
vhist = thishist;
}
if( sigs == 0 )
return 0;
int iteration;
float errbest = AttemptDecomposition( histogram, bins, out_means, out_amps, out_sigmas, sigs );
int dropped[bins];
for( i = 0; i < bins; i++ )
{
dropped[i] = 0;
}
for( iteration = 0; iteration < iterations; iteration++ )
{
if( dropped[iteration%sigs] ) continue;
//Tweak with the values until they are closer to what we want.
float backup_mean = out_means[iteration%sigs];
float backup_amps = out_amps[iteration%sigs];
float backup_sigmas = out_sigmas[iteration%sigs];
float mute = 20. / (iteration+20.);
#define RANDFN ((rand()%2)-0.5)*mute
//#define RANDFN ((rand()%10000)-5000.0) / 5000.0 * mute
// out_sigmas[iteration%sigs] += RANDFN;
out_means[iteration%sigs] += RANDFN;
out_amps[iteration%sigs] += RANDFN;
float err = AttemptDecomposition( histogram, bins, out_means, out_amps, out_sigmas, sigs );
if( err > errbest )
{
out_means[iteration%sigs] = backup_mean;
out_amps[iteration%sigs] = backup_amps;
out_sigmas[iteration%sigs] = backup_sigmas;
}
else
{
if( out_amps[iteration%sigs] < 0.01 )
{
dropped[iteration%sigs] = 1;
out_amps[iteration%sigs] = 0.0;
}
errbest = err;
}
}
// printf( "%f / %f = %f\n", origerr, errbest, origerr/errbest );
return sigs;
}
float CalcHistAt( float pt, int bins, float * out_means, float * out_amps, float * out_sigmas, int cur_dists )
{
int i;
float mark = 0.0;
for( i = 0; i < cur_dists; i++ )
{
float amp = out_amps[i];
float mean = out_means[i];
float var = out_sigmas[i];
float x = mean - pt;
if( x < - bins / 2 ) x += bins;
if( x > bins / 2 ) x -= bins;
float nrm = amp / (var * SQRT2PI ) * expf( - ( x * x ) / ( 2 * var * var ) );
mark += nrm;
}
return mark;
}
float AttemptDecomposition( float * histogram, int bins, float * out_means, float * out_amps, float * out_sigmas, int cur_dists )
{
int i, j;
float hist[bins];
memcpy( hist, histogram, sizeof(hist) );
for( i = 0; i < cur_dists; i++ )
{
float amp = out_amps[i];
float mean = out_means[i];
float var = out_sigmas[i];
for( j = 0; j < bins; j++ )
{
float x = mean - j;
if( x < - bins / 2 ) x += bins;
if( x > bins / 2 ) x -= bins;
float nrm = amp / (var * SQRT2PI ) * expf( - ( x * x ) / ( 2 * var * var ) );
hist[j] -= nrm;
}
}
float remain = 0;
for( j = 0; j < bins; j++ )
{
remain += hist[j] * hist[j];
}
return remain;
}
#endif