void AccelerateFFT<float>::setAudioFrameSize (int frameSize)
{
fftSize = frameSize;
fftSizeOver2 = fftSize / 2;
log2n = log2f (fftSize);
if (configured)
{
free (complexSplit.realp);
free (complexSplit.imagp);
vDSP_destroy_fftsetup (fftSetupFloat);
}
complexSplit.realp = (float*)malloc (fftSize * sizeof (float));
complexSplit.imagp = (float*)malloc (fftSize * sizeof (float));
fftSetupFloat = vDSP_create_fftsetup (log2n, FFT_RADIX2);
if (fftSetupFloat == nullptr)
{
// couldn't set up FFT
assert (false);
}
configured = true;
}
FFT* newFFT(int size)
{
FFT* fft = (FFT*)malloc(sizeof(FFT));
if (fft == NULL)
return NULL;
// Check if the size is a power of two
assert(POWER_OF_TWO(size));
fft->size = size;
fft->sizeOverTwo = fft->size / 2;
fft->normalize = 1.0 / (2.0 * fft->size);
// create fft setup
fft->logTwo = log2f(fft->size);
fft->fftSetup = vDSP_create_fftsetup(fft->logTwo, FFT_RADIX2);
if (fft->fftSetup == 0)
{
freeFFT(fft);
return NULL;
}
fft->window = NULL;
return fft;
}
PitchDetector::PitchDetector(unsigned int numberOfSamples, double sampleRate)
{
assert(numberOfSamples > 0);
assert(sampleRate > 0);
this->numberOfSamples = numberOfSamples;
this->sampleRate = sampleRate;
this->queueSize = DEFAULT_QUEUE_SIZE;
// this->FFTBuffer = new float(numberOfSamples);
this->FFTBuffer = (float *)calloc(sizeof(float), numberOfSamples);
// 1.窗函数
this->windowFunc = (float *)calloc(sizeof(float), numberOfSamples);
vDSP_hamm_window(windowFunc, numberOfSamples, 0);
// 2.fft处理buffer
this->A.realp = (float * __nonnull)malloc(sizeof(float)*(numberOfSamples/2));
this->A.imagp = (float * __nonnull)malloc(sizeof(float)*(numberOfSamples/2));
// 3.能量
this->magnitudes = (float *)malloc(sizeof(float)*(numberOfSamples/2));
// 4.fft变换的预设
float originalRealInLog2 = log2f(numberOfSamples);
this->setupReal = vDSP_create_fftsetup(originalRealInLog2, FFT_RADIX2);
}
void scfft_global_init(){
unsigned short wintype, i;
for (wintype=0; wintype<2; ++wintype) {
for (i=0; i< SC_FFT_LOG2_ABSOLUTE_MAXSIZE_PLUS1; ++i) {
fftWindow[wintype][i] = 0;
}
for (i= SC_FFT_LOG2_MINSIZE; i < SC_FFT_LOG2_MAXSIZE+1; ++i) {
fftWindow[wintype][i] = scfft_create_fftwindow(wintype, i);
}
}
#if SC_FFT_VDSP
// vDSP inits its twiddle factors
for (i= SC_FFT_LOG2_MINSIZE; i < SC_FFT_LOG2_MAXSIZE+1; ++i) {
fftSetup[i] = vDSP_create_fftsetup (i, FFT_RADIX2);
if(fftSetup[i] == NULL){
printf("FFT ERROR: Mac vDSP library could not allocate FFT setup for size %i\n", 1<<i);
}
}
// vDSP prepares its memory-aligned buffer for rearranging input data.
// Note max size here - meaning max input buffer size is these two sizes added together.
// vec_malloc used in API docs, but apparently that's deprecated and malloc is sufficient for aligned memory on OSX.
splitBuf.realp = (float*) malloc ( SC_FFT_MAXSIZE * sizeof(float) / 2);
splitBuf.imagp = (float*) malloc ( SC_FFT_MAXSIZE * sizeof(float) / 2);
//printf("SC FFT global init: vDSP initialised.\n");
#elif SC_FFT_FFTW
//printf("SC FFT global init: FFTW, no init needed.\n");
#endif
}
void ofxFFT::setup(int numFrames) {
this->numFrames = numFrames;
log2n = log2(numFrames);
mFFTLength = numFrames / 2;
mSpectrumAnalysis = vDSP_create_fftsetup(log2n, FFT_RADIX2);
if (mSpectrumAnalysis == NULL) {
printf("\nFFT_Setup failed to allocate enough memory for"
"the real FFT.\n");
//std::exit(0);
}
mDspSplitComplex.realp = new float[mFFTLength];
mDspSplitComplex.imagp = new float[mFFTLength];
input = new float[numFrames];
amplitude = new float[mFFTLength];
power = new float[mFFTLength];
mFFTNormFactor = 1.0/(2.0*numFrames);
counter = 0;
bReady = false;
}
// check the global list of windows incs ours; create if not.
// Note that expanding the table, if triggered, will cause a CPU hit as things are malloc'ed, realloc'ed, etc.
void scfft_ensurewindow(unsigned short log2_fullsize, unsigned short log2_winsize, short wintype)
{
// Ensure we have enough space to do our calcs
if(log2_fullsize > largest_log2n){
largest_log2n = log2_fullsize;
#if SC_FFT_VDSP
size_t newsize = (1 << largest_log2n) * sizeof(float) / 2;
splitBuf.realp = (float*) realloc (splitBuf.realp, newsize);
splitBuf.imagp = (float*) realloc (splitBuf.imagp, newsize);
#endif
}
// Ensure our window has been created
if((wintype != -1) && (fftWindow[wintype][log2_winsize] == 0)){
fftWindow[wintype][log2_winsize] = scfft_create_fftwindow(wintype, log2_winsize);
}
// Ensure our FFT twiddle factors (or whatever) have been created
#if SC_FFT_VDSP
if(fftSetup[log2_fullsize] == 0)
fftSetup[log2_fullsize] = vDSP_create_fftsetup (log2_fullsize, FFT_RADIX2);
#elif SC_FFT_GREEN
if(cosTable[log2_fullsize] == 0)
cosTable[log2_fullsize] = create_cosTable(log2_fullsize);
#endif
}
DspRfft::DspRfft(PdMessage *initMessage, PdGraph *graph) : DspObject(0, 1, 0, 2, graph) {
#if __APPLE__
log2n = lrintf(log2f(blockSizeFloat));
fftSetup = vDSP_create_fftsetup(log2n, kFFTRadix2);
#else
graph->printErr("[rfft~] is not supported on this platform. It is only supported on Apple OS X and iOS platforms.");
#endif // __APPLE__
}
FFT::FFT (int fftSizeLog2)
: properties (fftSizeLog2)
{
config = vDSP_create_fftsetup (properties.fftSizeLog2, 0);
buffer.malloc (properties.fftSize);
bufferSplit.realp = buffer.getData();
bufferSplit.imagp = bufferSplit.realp + properties.fftSizeHalved;
}
void CFFTOSX::Init(tuint uiOrder)
{
muiOrder = uiOrder;
muiFFTSize = Float2Int(pow(2, muiOrder));
mA.realp = new float[muiFFTSize / 2];
mA.imagp = new float[muiFFTSize / 2];
mFFT = vDSP_create_fftsetup(muiOrder, FFT_RADIX2);
}
FFTHelper::FFTHelper ( UInt32 inMaxFramesPerSlice )
: mSpectrumAnalysis(NULL),
mFFTNormFactor(1.0/(2*inMaxFramesPerSlice)),
mFFTLength(inMaxFramesPerSlice/2),
mLog2N(Log2Ceil(inMaxFramesPerSlice))
{
mDspSplitComplex.realp = (Float32*) calloc(mFFTLength,sizeof(Float32));
mDspSplitComplex.imagp = (Float32*) calloc(mFFTLength, sizeof(Float32));
mSpectrumAnalysis = vDSP_create_fftsetup(mLog2N, kFFTRadix2);
}
DspRfft::DspRfft(PdMessage *initMessage, PdGraph *graph) : DspObject(0, 1, 0, 2, graph) {
#if __APPLE__
log2n = lrintf(log2f((float) blockSizeInt));
fftSetup = vDSP_create_fftsetup(log2n, kFFTRadix2);
zeroBuffer = graph->getBufferPool()->getZeroBuffer(); // cache the local zero buffer
#else
graph->printErr("[rfft~] is not supported on this platform. It is only supported on Apple OS X and iOS platforms.");
#endif // __APPLE__
processFunction = &processSignal;
}
//Constructor
FFTAccelerate::FFTAccelerate (int numSamples)
{
vDSP_Length log2n = log2f(numSamples);
fftSetup = vDSP_create_fftsetup(log2n, FFT_RADIX2);
int nOver2 = numSamples/2;
A.realp = (float *) malloc(nOver2*sizeof(float));
A.imagp = (float *) malloc(nOver2*sizeof(float));
window = (float *) malloc(numSamples * sizeof(float));
in_real = (float *) malloc(numSamples * sizeof(float));
memset(window, 0, numSamples * sizeof(float));
vDSP_hann_window(window, numSamples, vDSP_HANN_NORM);
}
FftProcessorImplAccelerate::FftProcessorImplAccelerate( uint16_t aBandCount )
: FftProcessorImpl( aBandCount )
{
if( mBandCount & ( mBandCount - 1 ) ) {
//TODO: not power of 2
}
mLog2Size = log( mBandCount * 2 ) / log( 2 );
mFftSetup = vDSP_create_fftsetup( mLog2Size, FFT_RADIX2 ); //n = 512, log2n = 9
mFftComplexBuffer.realp = (float *)malloc( mBandCount * sizeof( float ) );
mFftComplexBuffer.imagp = (float *)malloc( mBandCount * sizeof( float ) );
}
FFTSetup FFTFrame::fftSetupForSize(unsigned fftSize) {
if (!fftSetups) {
fftSetups = (FFTSetup*)malloc(sizeof(FFTSetup) * kMaxFFTPow2Size);
memset(fftSetups, 0, sizeof(FFTSetup) * kMaxFFTPow2Size);
}
int pow2size = static_cast<int>(log2(fftSize));
ASSERT(pow2size < kMaxFFTPow2Size);
if (!fftSetups[pow2size])
fftSetups[pow2size] = vDSP_create_fftsetup(pow2size, FFT_RADIX2);
return fftSetups[pow2size];
}
void FFT::setFFTSizeLog2 (int newFFTSizeLog2)
{
if (newFFTSizeLog2 != properties.fftSizeLog2)
{
vDSP_destroy_fftsetup (config);
properties = Properties (newFFTSizeLog2);
buffer.malloc (properties.fftSize);
bufferSplit.realp = buffer.getData();
bufferSplit.imagp = bufferSplit.realp + properties.fftSizeHalved;
config = vDSP_create_fftsetup (properties.fftSizeLog2, 0);
}
}
dm_fftSetup dm_fftSetupCreate(unsigned long size_, dm_fftDirection direction_) {
fftsetup_internal* toReturn = new fftsetup_internal();
toReturn->direction = direction_;
#ifdef DSP_USE_ACCELERATE
toReturn->logFftSize = log2f(size_);
toReturn->fftSetup = vDSP_create_fftsetup(toReturn->logFftSize, FFT_RADIX2);
#else
toReturn->fftSize = size_;
toReturn->fftSetup = kiss_fftr_alloc(size_, (toReturn->direction == DM_FFT_FORWARD) ? 0 : 1, 0, 0);
toReturn->temp = new float[size_];
memset(toReturn->temp, 0, sizeof(float) * size_);
#endif
return toReturn;
}
// create a vDSP power spectrum structure
CMFFT *FFTSpectrum( int log2n, Boolean useWindow )
{
CMFFT *fft ;
fft = ( CMFFT*)malloc( sizeof( CMFFT ) ) ;
fft->style = PowerSpectrum ;
fft->log2n = log2n ;
fft->size = 1 << log2n ;
fft->window = ( useWindow ) ? CMMakeModifiedBlackmanWindow( fft->size/2 ) : nil ;
fft->z.realp = ( float* )malloc( fft->size*sizeof( float )/2 ) ;
fft->z.imagp = ( float* )malloc( fft->size*sizeof( float )/2 ) ;
fft->tempBuf.realp = ( float* )malloc( sizeof( float )*16384 ) ;
fft->tempBuf.imagp = ( float* )malloc( sizeof( float )*16384 ) ;
fft->vfft = vDSP_create_fftsetup( log2n, FFT_RADIX2 ) ;
return fft ;
}
FFTBufferManager::FFTBufferManager(UInt32 inNumberFrames) :
mNeedsAudioData(0),
mHasAudioData(0),
mFFTNormFactor(1.0/(2*inNumberFrames)),
mAdjust0DB(1.5849e-13),
m24BitFracScale(16777216.0f),
mFFTLength(inNumberFrames/2),
mLog2N(Log2Ceil(inNumberFrames)),
mNumberFrames(inNumberFrames),
mAudioBufferSize(inNumberFrames * sizeof(Float32)),
mAudioBufferCurrentIndex(0)
{
mAudioBuffer = (Float32*) calloc(mNumberFrames,sizeof(Float32));
mDspSplitComplex.realp = (Float32*) calloc(mFFTLength,sizeof(Float32));
mDspSplitComplex.imagp = (Float32*) calloc(mFFTLength, sizeof(Float32));
mSpectrumAnalysis = vDSP_create_fftsetup(mLog2N, kFFTRadix2);
OSAtomicIncrement32Barrier(&mNeedsAudioData);
}
void ofxAudioUnitFftNode::setFftBufferSize(unsigned int bufferSize)
{
_log2N = (unsigned int) ceilf(log2f(bufferSize));
_N = 1 << _log2N;
// if the new buffer size is bigger than what we've allocated for,
// free everything and allocate anew (otherwise re-use)
if(_log2N > _currentMaxLog2N) {
freeBuffers();
_fftData.realp = (float *)calloc(_N / 2, sizeof(float));
_fftData.imagp = (float *)calloc(_N / 2, sizeof(float));
_window = (float *)calloc(_N, sizeof(float));
_fftSetup = vDSP_create_fftsetup(_log2N, kFFTRadix2);
_currentMaxLog2N = _log2N;
}
generateWindow(_outputSettings.window, _window, _N);
setBufferSize(_N);
}
CMFFT *FFTForward( int log2n, Boolean useWindow )
{
CMFFT *fft ;
fft = ( CMFFT*)malloc( sizeof( CMFFT ) ) ;
fft->style = Forward ;
fft->log2n = log2n ;
fft->size = 1 << log2n ;
fft->window = ( useWindow ) ? CMMakeModifiedBlackmanWindow( fft->size ) : nil ;
fft->tempBuf.realp = (float*)malloc( sizeof(float)*16384 ) ;
fft->tempBuf.imagp = (float*)malloc( sizeof(float)*16384 ) ;
if ( useWindow ) {
fft->realBuf = (float*)malloc( sizeof(float)*fft->size ) ;
fft->imagBuf = (float*)malloc( sizeof(float)*fft->size ) ;
}
fft->vfft = vDSP_create_fftsetup( log2n, FFT_RADIX2 ) ;
return fft ;
}
// Demonstrate vDSP FFT functions.
void DemonstrateFFT(void)
{
printf("Begin %s.\n", __func__);
// Initialize data for the FFT routines.
FFTSetup Setup = vDSP_create_fftsetup(Log2N, FFT_RADIX2);
if (Setup == NULL)
{
fprintf(stderr, "Error, vDSP_create_fftsetup failed.\n");
exit (EXIT_FAILURE);
}
DemonstratevDSP_fft_zrip(Setup);
DemonstratevDSP_fft_zrop(Setup);
DemonstratevDSP_fft_zip(Setup);
DemonstratevDSP_fft_zop(Setup);
vDSP_destroy_fftsetup(Setup);
printf("\nEnd %s.\n\n\n", __func__);
}
/* constructor */
fft::fft(int fftSize) {
n = fftSize;
half = fftSize / 2;
//use malloc for 16 byte alignment
in_real = (float *) malloc(n * sizeof(float));
in_img = (float *) malloc(n * sizeof(float));
out_real = (float *) malloc(n * sizeof(float));
out_img = (float *) malloc(n * sizeof(float));
#ifdef __APPLE_CC__
log2n = log2(n);
A.realp = (float *) malloc(half * sizeof(float));
A.imagp = (float *) malloc(half * sizeof(float));
setupReal = vDSP_create_fftsetup(log2n, FFT_RADIX2);
if (setupReal == NULL) {
printf("\nFFT_Setup failed to allocate enough memory for"
"the real FFT.\n");
}
polar = (float *) malloc(n * sizeof(float));
#endif
}
FFT32 *FFT32_new(size_t FFTFrameSize, FFTWindowType windowType)
{
FFT32 *self = calloc(1, sizeof(FFT32));
if (FFTFrameSize < FFT_minimumFFTFrameSize) {
printf("FFTFrameSize specified is lower than minimum, defaulting to minimum\n");
FFTFrameSize = FFT_minimumFFTFrameSize;
}
self->FFTFrameSize = nextPowerOfTwo(FFTFrameSize);
self->FFTFrameSizeOver2 = self->FFTFrameSize / 2;
self->log2n = log2f((Float32)FFTFrameSize);
self->frameBuffer = calloc(self->FFTFrameSize, sizeof(Float32));
self->window = calloc(self->FFTFrameSize, sizeof(Float32));
self->interlacedPolar = calloc(self->FFTFrameSize, sizeof(Float32));
self->splitComplex.realp = calloc(self->FFTFrameSizeOver2, sizeof(Float32));
self->splitComplex.imagp = calloc(self->FFTFrameSizeOver2, sizeof(Float32));
self->FFTData = vDSP_create_fftsetup(self->log2n, FFT_RADIX2);
FFT32_configureWindow(self, windowType);
return self;
}
int main(int argc, char *argv[])
{
// Initialize the pseudo-random number generator.
InitializeRandom();
// Initialize FFT data.
FFTSetup Setup = vDSP_create_fftsetup(Log2SampleLength, FFT_RADIX2);
if (Setup == 0)
{
fprintf(stderr, "Error, unable to create FFT setup.\n");
exit(EXIT_FAILURE);
}
/* If there are no command-line arguments, prompt for keys
interactively.
*/
if (argc <= 1)
{
// Process keys for the user until they are done.
int c;
while (1)
{
// Prompt the user.
printf(
"Please enter a key (one of 0-9, *, #, or A-D): ");
// Skip whitespace except newlines.
do
c = getchar();
while (c != '\n' && isspace(c));
// When there is a blank line or an EOF, quit.
if (c == '\n' || c == EOF)
break;
// Look up the key in the table.
FrequencyPair F = ConvertKeyToFrequencies(toupper(c));
// If it is a valid key, demonstrate the FFT.
if (F.Frequency[0] != 0)
Demonstrate(Setup, F);
// Skip anything else on the line.
do
c = getchar();
while (c != EOF && c != '\n');
// When the input ends, quit. Otherwise, do more.
if (c == EOF)
break;
}
// Do not leave the cursor in the middle of a line.
if (c != '\n')
printf("\n");
}
// If there is one command line argument, process the keys in it.
else if (argc == 2)
{
for (char *p = argv[1]; *p; ++p)
{
// Look up the key in the table.
FrequencyPair F = ConvertKeyToFrequencies(toupper(*p));
// If it is a valid key, demonstrate the FFT.
if (F.Frequency[0] != 0)
{
printf("Simulating key %c.\n", *p);
Demonstrate(Setup, F);
}
else
fprintf(stderr,
"Error, key %c not recognized.\n", *p);
}
}
// If there are too many arguments, print a usage message.
else
{
fprintf(stderr,
"Usage: %s [telephone keys 0-9, #, *, or A-D]\n",
argv[0]);
exit(EXIT_FAILURE);
}
// Release resources.
vDSP_destroy_fftsetup(Setup);
return 0;
}
void benchmark_ffts(int N, int cplx) {
int Nfloat = (cplx ? N*2 : N);
int Nbytes = Nfloat * sizeof(float);
float *X = pffft_aligned_malloc(Nbytes), *Y = pffft_aligned_malloc(Nbytes), *Z = pffft_aligned_malloc(Nbytes);
double t0, t1, flops;
int k;
int max_iter = 5120000/N*4;
#ifdef __arm__
max_iter /= 4;
#endif
int iter;
for (k = 0; k < Nfloat; ++k) {
X[k] = 0; //sqrtf(k+1);
}
// FFTPack benchmark
{
float *wrk = malloc(2*Nbytes + 15*sizeof(float));
int max_iter_ = max_iter/pffft_simd_size(); if (max_iter_ == 0) max_iter_ = 1;
if (cplx) cffti(N, wrk);
else rffti(N, wrk);
t0 = uclock_sec();
for (iter = 0; iter < max_iter_; ++iter) {
if (cplx) {
cfftf(N, X, wrk);
cfftb(N, X, wrk);
} else {
rfftf(N, X, wrk);
rfftb(N, X, wrk);
}
}
t1 = uclock_sec();
free(wrk);
flops = (max_iter_*2) * ((cplx ? 5 : 2.5)*N*log((double)N)/M_LN2); // see http://www.fftw.org/speed/method.html
show_output("FFTPack", N, cplx, flops, t0, t1, max_iter_);
}
#ifdef HAVE_VECLIB
int log2N = (int)(log(N)/log(2) + 0.5f);
if (N == (1<<log2N)) {
FFTSetup setup;
setup = vDSP_create_fftsetup(log2N, FFT_RADIX2);
DSPSplitComplex zsamples;
zsamples.realp = &X[0];
zsamples.imagp = &X[Nfloat/2];
t0 = uclock_sec();
for (iter = 0; iter < max_iter; ++iter) {
if (cplx) {
vDSP_fft_zip(setup, &zsamples, 1, log2N, kFFTDirection_Forward);
vDSP_fft_zip(setup, &zsamples, 1, log2N, kFFTDirection_Inverse);
} else {
vDSP_fft_zrip(setup, &zsamples, 1, log2N, kFFTDirection_Forward);
vDSP_fft_zrip(setup, &zsamples, 1, log2N, kFFTDirection_Inverse);
}
}
t1 = uclock_sec();
vDSP_destroy_fftsetup(setup);
flops = (max_iter*2) * ((cplx ? 5 : 2.5)*N*log((double)N)/M_LN2); // see http://www.fftw.org/speed/method.html
show_output("vDSP", N, cplx, flops, t0, t1, max_iter);
} else {
show_output("vDSP", N, cplx, -1, -1, -1, -1);
}
#endif
#ifdef HAVE_FFTW
{
fftwf_plan planf, planb;
fftw_complex *in = (fftw_complex*) fftwf_malloc(sizeof(fftw_complex) * N);
fftw_complex *out = (fftw_complex*) fftwf_malloc(sizeof(fftw_complex) * N);
memset(in, 0, sizeof(fftw_complex) * N);
int flags = (N < 40000 ? FFTW_MEASURE : FFTW_ESTIMATE); // measure takes a lot of time on largest ffts
//int flags = FFTW_ESTIMATE;
if (cplx) {
planf = fftwf_plan_dft_1d(N, (fftwf_complex*)in, (fftwf_complex*)out, FFTW_FORWARD, flags);
planb = fftwf_plan_dft_1d(N, (fftwf_complex*)in, (fftwf_complex*)out, FFTW_BACKWARD, flags);
} else {
planf = fftwf_plan_dft_r2c_1d(N, (float*)in, (fftwf_complex*)out, flags);
planb = fftwf_plan_dft_c2r_1d(N, (fftwf_complex*)in, (float*)out, flags);
}
t0 = uclock_sec();
for (iter = 0; iter < max_iter; ++iter) {
fftwf_execute(planf);
fftwf_execute(planb);
}
t1 = uclock_sec();
fftwf_destroy_plan(planf);
fftwf_destroy_plan(planb);
fftwf_free(in); fftwf_free(out);
flops = (max_iter*2) * ((cplx ? 5 : 2.5)*N*log((double)N)/M_LN2); // see http://www.fftw.org/speed/method.html
show_output((flags == FFTW_MEASURE ? "FFTW (meas.)" : " FFTW (estim)"), N, cplx, flops, t0, t1, max_iter);
}
#endif
// PFFFT benchmark
{
PFFFT_Setup *s = pffft_new_setup(N, cplx ? PFFFT_COMPLEX : PFFFT_REAL);
if (s) {
t0 = uclock_sec();
for (iter = 0; iter < max_iter; ++iter) {
pffft_transform(s, X, Z, Y, PFFFT_FORWARD);
pffft_transform(s, X, Z, Y, PFFFT_BACKWARD);
}
t1 = uclock_sec();
pffft_destroy_setup(s);
flops = (max_iter*2) * ((cplx ? 5 : 2.5)*N*log((double)N)/M_LN2); // see http://www.fftw.org/speed/method.html
show_output("PFFFT", N, cplx, flops, t0, t1, max_iter);
}
}
if (!array_output_format) {
printf("--\n");
}
pffft_aligned_free(X);
pffft_aligned_free(Y);
pffft_aligned_free(Z);
}
void computeReferenceF(clFFT_SplitComplex *out, clFFT_Dim3 n,
unsigned int batchSize, clFFT_Dimension dim, clFFT_Direction dir)
{
FFTSetup plan_vdsp;
DSPSplitComplex out_vdsp;
FFTDirection dir_vdsp = dir == clFFT_Forward ? FFT_FORWARD : FFT_INVERSE;
unsigned int i, j, k;
unsigned int stride;
unsigned int log2Nx = (unsigned int) log2(n.x);
unsigned int log2Ny = (unsigned int) log2(n.y);
unsigned int log2Nz = (unsigned int) log2(n.z);
unsigned int log2N;
log2N = log2Nx;
log2N = log2N > log2Ny ? log2N : log2Ny;
log2N = log2N > log2Nz ? log2N : log2Nz;
plan_vdsp = vDSP_create_fftsetup(log2N, 2);
switch(dim)
{
case clFFT_1D:
for(i = 0; i < batchSize; i++)
{
stride = i * n.x;
out_vdsp.realp = out->real + stride;
out_vdsp.imagp = out->imag + stride;
vDSP_fft_zip(plan_vdsp, &out_vdsp, 1, log2Nx, dir_vdsp);
}
break;
case clFFT_2D:
for(i = 0; i < batchSize; i++)
{
for(j = 0; j < n.y; j++)
{
stride = j * n.x + i * n.x * n.y;
out_vdsp.realp = out->real + stride;
out_vdsp.imagp = out->imag + stride;
vDSP_fft_zip(plan_vdsp, &out_vdsp, 1, log2Nx, dir_vdsp);
}
}
for(i = 0; i < batchSize; i++)
{
for(j = 0; j < n.x; j++)
{
stride = j + i * n.x * n.y;
out_vdsp.realp = out->real + stride;
out_vdsp.imagp = out->imag + stride;
vDSP_fft_zip(plan_vdsp, &out_vdsp, n.x, log2Ny, dir_vdsp);
}
}
break;
case clFFT_3D:
for(i = 0; i < batchSize; i++)
{
for(j = 0; j < n.z; j++)
{
for(k = 0; k < n.y; k++)
{
stride = k * n.x + j * n.x * n.y + i * n.x * n.y * n.z;
out_vdsp.realp = out->real + stride;
out_vdsp.imagp = out->imag + stride;
vDSP_fft_zip(plan_vdsp, &out_vdsp, 1, log2Nx, dir_vdsp);
}
}
}
for(i = 0; i < batchSize; i++)
{
for(j = 0; j < n.z; j++)
{
for(k = 0; k < n.x; k++)
{
stride = k + j * n.x * n.y + i * n.x * n.y * n.z;
out_vdsp.realp = out->real + stride;
out_vdsp.imagp = out->imag + stride;
vDSP_fft_zip(plan_vdsp, &out_vdsp, n.x, log2Ny, dir_vdsp);
}
}
}
for(i = 0; i < batchSize; i++)
{
for(j = 0; j < n.y; j++)
{
for(k = 0; k < n.x; k++)
{
stride = k + j * n.x + i * n.x * n.y * n.z;
out_vdsp.realp = out->real + stride;
out_vdsp.imagp = out->imag + stride;
vDSP_fft_zip(plan_vdsp, &out_vdsp, n.x*n.y, log2Nz, dir_vdsp);
}
}
}
break;
}
vDSP_destroy_fftsetup(plan_vdsp);
}
//Processes a frame and returns output image
bool HiLight::processFrame(const cv::Mat& inputFrame, int* screen_position, double current_timestamp, char *results, bool* denoise_check, bool* first_bit_check, int* hilight_line_index, char* hilight_results_tmp )
{
*denoise_check = false;
*first_bit_check = false;
if(!initialized){
initialized = true;
for(int i = 0; i<grid_x; i++){
for(int j = 0; j<grid_y; j++ ){
for (int k = 0; k < first_bit_window; k++){
first_bit_color_window[i][j][k] = 0;
}
for (int k = 0; k < N; k++){
grid_color_intensity[i][j][k] = 0;
hilight_results_stack[i][j][k] = 0;
}
hilight_results[i][j] = 0;
}
}
if (isImage){
MVDR = true;
first_2_ratio = 0.6;//0.8
first_bit_voting = 0.5;//0.7
}else{
MVDR = false;
first_2_ratio = 0.8; //0.7
first_bit_voting = 0.7; //0.6
}
window_index = 0;
first_bit_detected = false;
first_bit_index = 0;
hilight_stack_index = 0;
start_time = current_timestamp;
current_time = current_timestamp;
denoise_start = false;
MVDR_index = 0;
bit_counter = 0;
results_stack_counter = 0;
fftSetup = vDSP_create_fftsetup(LOG_N, kFFTRadix2);
start_receiving = false;
counter_after_detect_1= 0;
first_bit_1_detected = false;
first_bit_2_detected = false;
hilight_first_bit_index = 0;
hilight_first_bit_counter = 0;
hilight_first_bit_position = 0;
if (screen_position[2] < screen_position[4]){
image_ROI_position[0].y = screen_position[2];
}else{
image_ROI_position[0].y = screen_position[4];
}
if (screen_position[1] < screen_position[7]){
image_ROI_position[0].x = screen_position[1];
}else{
image_ROI_position[0].x = screen_position[7];
}
if (screen_position[6] < screen_position[8]){
image_ROI_position[1].y = screen_position[8];
}else{
image_ROI_position[1].y = screen_position[6];
}
if (screen_position[5] < screen_position[3]){
image_ROI_position[1].x = screen_position[3];
}else{
image_ROI_position[1].x = screen_position[5];
}
if (isImage){
// Set grids points to calculate the grid position
for (int i = 0; i <= grid_x * grid_x_MVDR; i++){
grids_points_top_image[i].x = screen_position[1] + (screen_position[3] - screen_position[1]) / grid_x / grid_x_MVDR * i;
grids_points_top_image[i].y = screen_position[2] + (screen_position[4] - screen_position[2]) / grid_x / grid_x_MVDR * i;
grids_points_bottom_image[i].x = screen_position[7] + (screen_position[5] - screen_position[7]) / grid_x / grid_x_MVDR * i;
grids_points_bottom_image[i].y = screen_position[8] + (screen_position[6] - screen_position[8]) / grid_x / grid_x_MVDR * i;
}
for (int i = 0; i <= grid_y * grid_y_MVDR; i++){
grids_points_left_image[i].x = screen_position[1] + (screen_position[7] - screen_position[1]) / grid_y / grid_y_MVDR * i;
grids_points_left_image[i].y = screen_position[2] + (screen_position[8] - screen_position[2]) / grid_y / grid_y_MVDR * i;
grids_points_right_image[i].x = screen_position[3] + (screen_position[5] - screen_position[3]) / grid_y / grid_y_MVDR * i;
grids_points_right_image[i].y = screen_position[4] + (screen_position[6] - screen_position[4]) / grid_y / grid_y_MVDR * i;
}
for (int i = 0; i < grid_x * grid_x_MVDR; i++){
for (int j = 0; j < grid_y * grid_y_MVDR; j++){
cv::Point2f r1;
cv::Point2f r2;
cv::Point2f r3;
cv::Point2f r4;
intersection(grids_points_top_image[i], grids_points_bottom_image[i], grids_points_left_image[j], grids_points_right_image[j], r1);//top left
intersection(grids_points_top_image[i+1], grids_points_bottom_image[i+1], grids_points_left_image[j], grids_points_right_image[j], r2);//top right
intersection(grids_points_top_image[i+1], grids_points_bottom_image[i+1], grids_points_left_image[j+1], grids_points_right_image[j+1], r3);// bottom right
intersection(grids_points_top_image[i], grids_points_bottom_image[i], grids_points_left_image[j+1], grids_points_right_image[j+1], r4);//bottom left
//refine grid_position
if (r1.x <= r4.x){
grids_position_image[i][j][0].x = r4.x - image_ROI_position[0].x;
}else{
grids_position_image[i][j][0].x = r1.x - image_ROI_position[0].x;
}
if (r1.y <= r2.y){
grids_position_image[i][j][0].y = r2.y - image_ROI_position[0].y;
}else{
grids_position_image[i][j][0].y = r1.y - image_ROI_position[0].y;
}
if (r3.x <= r2.x){
grids_position_image[i][j][1].x = r3.x - image_ROI_position[0].x;
}else{
grids_position_image[i][j][1].x = r2.x - image_ROI_position[0].x;
}
if (r3.y <= r4.y){
grids_position_image[i][j][1].y = r3.y - image_ROI_position[0].y;
}else{
grids_position_image[i][j][1].y = r4.y - image_ROI_position[0].y;
}
}
}
}else{
// Set grids points to calculate the grid position
for (int i = 0; i <= grid_x; i++){
grids_points_top_video[i].x = screen_position[1] + (screen_position[3] - screen_position[1]) / grid_x * i;
grids_points_top_video[i].y = screen_position[2] + (screen_position[4] - screen_position[2]) / grid_x * i;
grids_points_bottom_video[i].x = screen_position[7] + (screen_position[5] - screen_position[7]) / grid_x * i;
grids_points_bottom_video[i].y = screen_position[8] + (screen_position[6] - screen_position[8]) / grid_x * i;
}
for (int i = 0; i <= grid_y; i++){
grids_points_left_video[i].x = screen_position[1] + (screen_position[7] - screen_position[1]) / grid_y * i;
grids_points_left_video[i].y = screen_position[2] + (screen_position[8] - screen_position[2]) / grid_y * i;
grids_points_right_video[i].x = screen_position[3] + (screen_position[5] - screen_position[3]) / grid_y * i;
grids_points_right_video[i].y = screen_position[4] + (screen_position[6] - screen_position[4]) / grid_y * i;
}
for (int i = 0; i < grid_x; i++){
for (int j = 0; j < grid_y; j++){
cv::Point2f r1;
cv::Point2f r2;
cv::Point2f r3;
cv::Point2f r4;
intersection(grids_points_top_video[i], grids_points_bottom_video[i], grids_points_left_video[j], grids_points_right_video[j], r1);//top left
intersection(grids_points_top_video[i+1], grids_points_bottom_video[i+1], grids_points_left_video[j], grids_points_right_video[j], r2);//top right
intersection(grids_points_top_video[i+1], grids_points_bottom_video[i+1], grids_points_left_video[j+1], grids_points_right_video[j+1], r3);// bottom right
intersection(grids_points_top_video[i], grids_points_bottom_video[i], grids_points_left_video[j+1], grids_points_right_video[j+1], r4);//bottom left
//refine grid_position
if (r1.x <= r4.x){
grids_position_video[i][j][0].x = r4.x - image_ROI_position[0].x;
}else{
grids_position_video[i][j][0].x = r1.x - image_ROI_position[0].x;
}
if (r1.y <= r2.y){
grids_position_video[i][j][0].y = r2.y - image_ROI_position[0].y;
}else{
grids_position_video[i][j][0].y = r1.y - image_ROI_position[0].y;
}
if (r3.x <= r2.x){
grids_position_video[i][j][1].x = r3.x - image_ROI_position[0].x;
}else{
grids_position_video[i][j][1].x = r2.x - image_ROI_position[0].x;
}
if (r3.y <= r4.y){
grids_position_video[i][j][1].y = r3.y - image_ROI_position[0].y;
}else{
grids_position_video[i][j][1].y = r4.y - image_ROI_position[0].y;
}
}
}
}
srcTri[0] = cv::Point2f( screen_position[2],screen_position[1] );
srcTri[1] = cv::Point2f( screen_position[4],screen_position[3] );
srcTri[2] = cv::Point2f( screen_position[6],screen_position[5] );
dist_top = sqrt(pow(screen_position[4]-screen_position[2],2.0) + pow(screen_position[3]-screen_position[1],2.0));
dstTri[0] = cv::Point2f( screen_position[2],screen_position[1] );
dstTri[1] = cv::Point2f( screen_position[2],screen_position[1] + dist_top );
dstTri[2] = cv::Point2f( screen_position[2] + (int)(dist_top*transmitter_screen_ratio),screen_position[1] + dist_top);
warp_mat = getAffineTransform( srcTri, dstTri );
}else{
current_time = current_timestamp;
}
//crop the transmitter screen from the captured image
cv::Mat image_ROI = inputFrame(cv::Range(image_ROI_position[0].y, image_ROI_position[1].y), cv::Range(image_ROI_position[0].x, image_ROI_position[1].x));
getGray_HiLight(image_ROI, grayImage);
image_ROI.release();
for (int c = 0; c < grid_x; c++){
for (int r = 0; r < grid_y; r++){
brightness[c][r] = 0;
}
}
if (isImage){
int brightness_c = 0;
int brightness_r = 0;
for (int c = 0; c < grid_x * grid_x_MVDR; c++){
for (int r = 0; r < grid_y * grid_y_MVDR; r++)
{
cv::Mat tile = grayImage(cv::Range(grids_position_image[c][r][0].y + grid_margin, grids_position_image[c][r][1].y - grid_margin)
, cv::Range(grids_position_image[c][r][0].x + grid_margin, grids_position_image[c][r][1].x - grid_margin));
cv::Scalar pixel_scalar = cv::mean(tile);
tile.release();
float brightness_tmp =pixel_scalar[0];
if (MVDR){
if(!denoise_start && MVDR_index < MVDR_frame){
grid_color_intensity_MVDR[c][r][MVDR_index] = brightness_tmp;
if ( c == grid_x * grid_x_MVDR -1 && r == grid_y * grid_y_MVDR - 1){
MVDR_index++;
}
}else if(!denoise_start && MVDR_index >= MVDR_frame){
denoise_start = true;
*denoise_check = true;
get_MVDR_weighting(grid_color_intensity_MVDR, MVDR_weighting);
//print MVDR matrix
if (isDebug){
for (int i = 0; i < grid_x * grid_x_MVDR; i++){
for (int j = 0; j < grid_y * grid_y_MVDR; j++){
printf("%f\t", MVDR_weighting[i][j]);
}
printf("\n");
}
}
}else{
brightness_c = floor(c*1.0/grid_x_MVDR);
brightness_r = floor(r*1.0/grid_y_MVDR);
brightness[brightness_c][brightness_r] = brightness[brightness_c][brightness_r] + brightness_tmp * MVDR_weighting[c][r];
}
}else{
brightness_c = floor(c*1.0/grid_x_MVDR);
brightness_r = floor(r*1.0/grid_y_MVDR);
brightness[brightness_c][brightness_r] = brightness[brightness_c][brightness_r] + brightness_tmp;
}
}
}
}else{
for (int c = 0; c < grid_x; c++){
for (int r = 0; r < grid_y; r++)
{
cv::Mat tile = grayImage(cv::Range(grids_position_video[c][r][0].y + grid_margin, grids_position_video[c][r][1].y - grid_margin)
, cv::Range(grids_position_video[c][r][0].x + grid_margin, grids_position_video[c][r][1].x - grid_margin));
cv::Scalar pixel_scalar = cv::mean(tile);
tile.release();
float brightness_tmp =pixel_scalar[0];
brightness[c][r] = brightness_tmp;
}
}
}
if (denoise_start || !MVDR){
for (int c = 0; c < grid_x; c++){
for (int r = 0; r < grid_y; r++){
if (window_index<(N-1)) {
grid_color_intensity[c][r][window_index] = brightness[c][r];
window_index++;
}else{
int i;
float fft_sum_N_over_2 = 0;
for (i = 1; i < N; i++){
grid_color_intensity[c][r][i-1] = grid_color_intensity[c][r][i];
}
grid_color_intensity[c][r][N-1] = brightness[c][r];
for (i = 0; i < N/2; i++){
fft_sum_N_over_2 = fft_sum_N_over_2 + grid_color_intensity[c][r][2*i] - grid_color_intensity[c][r][2*i+1];
}
// Initialize the input buffer with a sinusoid
// We need complex buffers in two different formats!
tempSplitComplex.realp = new float[N/2];
tempSplitComplex.imagp = new float[N/2];
// ----------------------------------------------------------------
// Forward FFT
// Scramble-pack the real data into complex buffer in just the way that's
// required by the real-to-complex FFT function that follows.
vDSP_ctoz((DSPComplex*)grid_color_intensity[c][r], 2, &tempSplitComplex, 1, N/2);
// Do real->complex forward FFT
vDSP_fft_zrip(fftSetup, &tempSplitComplex, 1, LOG_N, kFFTDirection_Forward);
int max_pulse_index = object_pulse_threashold;
float max_pulse = -100;
float object_pulse_power_1 = LinearToDecibel(pow (fft_sum_N_over_2, 2.0)/ N);
float object_pulse_power_2;
int object_pulse_force;
int object_pulse;
for (int k = object_pulse_threashold; k < N/2; k++)
{
float spectrum = LinearToDecibel((pow (tempSplitComplex.realp[k], 2.0) + pow (tempSplitComplex.imagp[k], 2.0))/ 4/ N);
if (max_pulse<spectrum){
max_pulse = spectrum;
max_pulse_index = k;
}
if(k == object_pulse_2){
object_pulse_power_2 = spectrum;
}
}
delete tempSplitComplex.realp;
delete tempSplitComplex.imagp;
if(max_pulse < object_pulse_power_1 && object_pulse_power_1 > -25){
object_pulse = 1;
}else if(max_pulse == object_pulse_power_2){
object_pulse = 2;
}else{
object_pulse = 0;
}
if(object_pulse_power_1 >= object_pulse_power_2){
object_pulse_force = 1;
}else{
object_pulse_force = 2;
}
//decode data by find the peak frequency power
if (first_bit_detected){
if (first_bit_1_detected && isCalibrate){
hilight_first_bit_stack[c][r][hilight_first_bit_index] = object_pulse_force;
}
if(current_time - start_time >= N / 60.0 * (bit_counter + 1)){
hilight_results[c][r] = get_hilight_results(hilight_results_stack[c][r], hilight_stack_index);
hilight_results_stack[c][r][0] = object_pulse_force;
}else{
hilight_results_stack[c][r][hilight_stack_index] = object_pulse_force;
}
}else{
first_bit_color_window[c][r][first_bit_index] = object_pulse;
}
}
}
}
if (first_bit_1_detected && isCalibrate){
hilight_first_bit_timestamp[hilight_first_bit_index++] = current_time;
}if(first_bit_2_detected && isCalibrate){
first_bit_2_detected = false;
hilight_first_bit_counter++;
if (hilight_first_bit_counter == N/2){
calibrate_first_bit_position();
}
}
if (first_bit_detected){
if(current_time - start_time >= N / 60.0 * (bit_counter + 1)){
int counter_for_2 = 0;
for (int i = 0; i < grid_x; i++){
for (int j = 0; j < grid_y; j++){
if (isDebug || start_receiving){
printf("%d ",hilight_results[i][j]);
}
if (hilight_results[i][j] == 2){
counter_for_2++;
}
}
}
if (isDebug || start_receiving){
printf("\n");
}
hilight_stack_index = 1;
bit_counter++;
if (!start_receiving){
counter_after_detect_1++;
if (counter_after_detect_1 > 3){
reset();
}else{
if(counter_for_2 >= (double)grid_x * grid_y * first_2_ratio){
first_bit_2_detected = true;
start_receiving = true;
*first_bit_check = true;
if (isDebug){
printf("\n");
}
}
}
return false;
}else{
int results_counter_tmp =0;
for (int i = 0; i < grid_x; i++){
for (int j = 0; j < grid_y; j++){
output_bit_stack[results_stack_counter++] = hilight_results[i][j];
hilight_results_tmp[results_counter_tmp++] = hilight_results[i][j] + '0' - 1;
}
}
hilight_line_index[0] = results_stack_counter/grid_x/grid_y;
if (results_stack_counter == output_bit_stck_length){
results_stack_counter = 0;
get_char_from_bits(output_bit_stack, results);
printf("%s\n",results);
if (demo){
debug_reset();
return true;
}else{
debug_reset();
return false;
}
}else{
return false;
}
}
}else{
hilight_stack_index++;
}
}else{
if(first_bit_index<first_bit_window-1){
first_bit_index++;
}else{
first_bit_detected = detect_first_bit(first_bit_color_window);
if (first_bit_detected){
if (isCalibrate){
first_bit_1_detected = true;
}
start_time = current_time;
}
}
}
}
return false;
}
