Skip to content

Instantly share code, notes, and snippets.

@mikusp
Created January 9, 2014 15:26
Show Gist options
  • Save mikusp/8335827 to your computer and use it in GitHub Desktop.
Save mikusp/8335827 to your computer and use it in GitHub Desktop.
Constant Q transform implemented in C using FFTW and custom build of CXSparse from SuiteSparse. Based on a MATLAB implementation presented in http://doc.ml.tu-berlin.de/bbci/material/publications/Bla_constQ.pdf .
#include <stdlib.h>
#include <math.h>
#include <complex.h>
#include "../../cxsparse/Include/cs.h"
#include <fftw3.h>
float* cq_hanning_window(int length) {
float* result = fftwf_alloc_real(length);
for (int j = 0; j < length; ++j) {
float elem = 0.5f * (1-(cos(j*2*M_PI / (float)(length - 1))));
result[j] = elem / (float)length;
}
return result;
}
fftwf_complex* cq_temp_kernel(float Q, int length) {
fftwf_complex* result = fftwf_alloc_complex(length);
for (int j = 0; j < length; ++j) {
fftwf_complex elem = cexpf(2*M_PI*_Complex_I*Q*j / (float)length);
result[j] = elem;
}
return result;
}
void cq_multiply_vector_elem(const float* in, const fftwf_complex* in2,
fftwf_complex* out, int length) {
for (int j = 0; j < length; ++j) {
fftwf_complex elem = in[j] * in2[j];
out[j] = elem;
}
}
float cq_q(int bins) {
return 1 / (exp2(1 / (float)bins) - 1);
}
int cq_k(int min_freq, int max_freq, int bins) {
return rint(ceil(bins * log2((float)max_freq / (float)min_freq)));
}
int cq_fft_len(float q, int min_freq, int sample_rate) {
return rint(exp2(ceil(log2(q * sample_rate / (float) min_freq))));
}
void cq_swap_matrix_column(float _Complex* matrix, int height, int width,
const float _Complex* vector, int index) {
for (int j = 0; j < height; ++j) {
matrix[j * width + index] = vector[j];
}
}
cs_ci* cq_make_kernel(int min_freq, int max_freq, int sample_rate,
int bins, int* height, int* width) {
float Q = cq_q(bins);
int K = cq_k(min_freq, max_freq, bins);
int fft = cq_fft_len(Q, min_freq, sample_rate);
fftwf_complex* temp = fftwf_alloc_complex(fft);
for (int j = 0; j < fft; ++j)
temp[j] = 0.f + 0.f*_Complex_I;
fftwf_complex* spec = fftwf_alloc_complex(fft);
cs_ci* result = cs_ci_spalloc(fft, K, (int)(fft*K*0.01f), 1, 1);
fftwf_plan plan = fftwf_plan_dft_1d(fft, temp, spec, FFTW_FORWARD,
FFTW_ESTIMATE);
for (int j = K-1; j >= 0; --j) {
int len = ceil(Q*sample_rate/(min_freq*exp2(j/(float)bins)));
float* h = cq_hanning_window(len);
fftwf_complex* vec = cq_temp_kernel(Q, len);
// temp = h .* vec
cq_multiply_vector_elem(h, vec, temp, len);
// spec = fft(temp)
fftwf_execute(plan);
for (int k = 0; k < fft; ++k) {
fftwf_complex temp = spec[k];
if (cabsf(temp) > 0.0054f) {
cs_ci_entry(result, k, j, conjf(temp) / (float)fft);
}
}
fftwf_free(h);
fftwf_free(vec);
}
fftwf_destroy_plan(plan);
fftwf_free(temp);
fftwf_free(spec);
cs_ci* ret = cs_ci_compress(result);
cs_ci_spfree(result);
*height = fft;
*width = K;
return ret;
}
fftwf_complex* cq_const_q_transform(float* data, const cs_ci* kernel,
int height, int width, const fftwf_plan plan, fftwf_complex* buffer) {
fftwf_execute_dft_r2c(plan, data, buffer);
// fill redundant data
for (int j = 0; j < height / 2 - 1; ++j) {
buffer[height - j - 1] = conjf(buffer[j + 1]);
}
fftwf_complex* result = fftwf_alloc_complex(width);
for (int j = 0; j < width; ++j) {
result[j] = 0.0f;
}
cs_ci_gaxpy(kernel, buffer, result);
return result;
}
fftwf_complex* cq_const_q_wrap(float* data, const cs_ci* kernel, int height,
int width, const int* indices, int indices_size) {
fftwf_complex* temp_fft = fftwf_alloc_complex(height);
fftwf_plan plan = fftwf_plan_dft_r2c_1d(height, data, temp_fft, FFTW_ESTIMATE |
FFTW_PRESERVE_INPUT);
fftwf_complex* result = fftwf_alloc_complex(height * indices_size);
for (int j = 0; j < indices_size; ++j) {
fftwf_complex* cq = cq_const_q_transform(data + indices[j], kernel,
height, width, plan, temp_fft);
cq_swap_matrix_column(result, width, indices_size, cq, j);
fftwf_free(cq);
}
fftwf_free(temp_fft);
fftwf_destroy_plan(plan);
return result;
}
fftwf_complex* cq_short_time_constq_transform(float* data, int data_length,
int min_freq, int max_freq, int sample_rate, int bins, int step,
int* height, int* width) {
int kernel_height, kernel_width;
cs_ci* ker = cq_make_kernel(min_freq, max_freq, sample_rate, bins,
&kernel_height, &kernel_width);
cs_ci* kern = cs_ci_transpose(ker, 1);
int max_index = rint(ceil(data_length / (float)kernel_height));
int indices_size = (max_index - 1) * kernel_height / (double) step + 1;
int* indices = (int*) malloc(indices_size * sizeof(int));
for (int j = 0, k = 0; j <= (max_index - 1) * kernel_height; ++k, j += step)
indices[k] = j;
fftwf_complex* result = cq_const_q_wrap(data, kern, kernel_height,
kernel_width, indices, indices_size);
cs_ci_spfree(ker);
cs_ci_spfree(kern);
free(indices);
*height = kernel_width;
*width = indices_size;
return result;
}
#ifndef CQ_FFT_H
#define CQ_FFT_H
#include <complex.h>
#include <cs.h>
#include <fftw3.h>
/**
* \brief Prints given vector on stdout
*
* Each value is printed up to 7 decimal places
*
* \param vec Vector to be printed
* \param length Length of a given vector
*/
void cq_dump_vector(const double* vec, int length);
/**
* \brief Prints given complex vector on stdout
*
* Prints "Re(val) + Im(val)i", each up to 7 decimal places
*
* \param vec Complex vector to be printed
* \param length Length of a given vector
*/
void cq_dump_vector_complex(const double _Complex* vec, int length);
/**
* \brief Prints given matrix to stdout
*
* Prints magnitude of each value in a matrix
*
* \param matrix Matrix to be printed
* \param height
* \param width
*/
void cq_dump_matrix_abs(double _Complex* matrix, int height, int width);
/**
* \brief Returns hanning window of a given length
*
* Returned vector is normalized by dividing each element by its length
*
* \param length length of a returned window
*/
double* cq_hanning_window(int length);
/**
* \brief Returns temporary vector used in computing a transform kernel
*
* \param Q Q value in transform
* \param length
*/
fftw_complex* cq_temp_kernel(double Q, int length);
/**
* \brief Helper function to compute x = a .* b, where x, a, b are vectors
*
* \param in
* \param in2
* \param out Vector to put result in, have to be allocated
* \param length Length of each vector
*/
void cq_multiply_vector_elem(const double* in, const fftw_complex* in2,
fftw_complex* out, int length);
/**
* \brief Computes Q coefficient
*
* \param bins Number of bins per octave
*/
double cq_q(int bins);
/**
* \brief Computes K coefficient
*
* \param min_freq Lower bound of frequency range
* \param max_freq Higher bound of frequency range
* \param bins Number of bins per octave
*/
int cq_k(int min_freq, int max_freq, int bins);
/**
* \brief Computes length of FFT used with given transform parameters
*
* \param q Q coefficient returned by cq_q(int)
* \param min_freq Lower bound of frequency range
* \param sample_rate Sample rate of source signal
*/
int cq_fft_len(double q, int min_freq, int sample_rate);
/**
* \brief Zeroes values which amplitude is smaller than threshold
*
* \param vec Input vector
* \param length Length of a vector
* \param thresh Threshold
*/
void cq_zero_vector_below_thresh(double _Complex* vec, int length,
double thresh);
/**
* \brief Sets values in a given column of a given matrix to those in a vector
*
* \param matrix
* \param height Height of a matrix
* \param width Width of a matrix
* \param vector Vector of values
* \param index Index of a column in matrix
*/
void cq_swap_matrix_column(double _Complex* matrix, int height, int width,
const double _Complex* vector, int index);
/**
* \brief Returns a transform kernel used throughout the algorithm
*
* This kernel is a sparse matrix, so it returns a compressed column storage
* form that is efficient in arithmetical operations.
*
* \param min_freq Lower bound of frequency range
* \param max_freq Higher bound of frequency range
* \param sample_rate Sample rate of a source signal
* \param bins Number of bins per octave
* \param[out] height Height of a returned matrix
* \param[out] width Width of a returned matrix
*/
cs_ci* cq_make_kernel(int min_freq, int max_freq, int sample_rate, int bins,
int* height, int* width);
/**
* \brief Computes constant Q transform of a given data
*
* \param data Source data for computation
* \param kernel Kernel obtained from cq_make_kernel
* \param height Height of a kernel
* \param width Width of a kernel
* \param plan FFTW plan for computing FFT, it has to be of R2C kind
* \param buffer Temporary buffer for computation, reused for optimization
*/
fftw_complex* cq_const_q_transform(double* data, const cs_ci* kernel,
int height, int width, const fftw_plan plan, fftw_complex* buffer);
/**
* \brief Wraps constant Q transform function
*
* Creates and deletes FFTW's plan and buffer for optimization purposes
*
* \param data Source data for computation
* \param kernel Kernel obtained from cq_make_kernel
* \param height Height of a kernel
* \param width Width of a kernel
* \param[in] indices Array with consecutive offsets in data
* \param indices_size Size of indices array
*/
fftw_complex* cq_const_q_wrap(double* data, const cs_ci* kernel, int height,
int width, const int* indices, int indices_size);
/**
* \brief Computes short time constant Q transform
*
* Tested only with arrays which length is a power of 2.
*
* \param data Source data
* \param data_length Length of source data
* \param min_freq Lower bound of frequency range
* \param max_freq Higher bound of frequency range
* \param sample_rate Sample rate of given signal
* \param bins Number of bins per octave
* \param step Difference between consecutive slices of data used in the algorithm
* \param[out] height Height of a returned matrix
* \param[out] width Width of a returned matrix
*/
fftw_complex* cq_short_time_constq_transform(double* data, int data_length,
int min_freq, int max_freq, int sample_rate, int bins, int step,
int* height, int* width);
#endif /* CQ_FFT_H */
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment