qm-dsp  1.8
Resampler.cpp
Go to the documentation of this file.
1 /* -*- c-basic-offset: 4 indent-tabs-mode: nil -*- vi:set ts=8 sts=4 sw=4: */
2 /*
3  QM DSP Library
4 
5  Centre for Digital Music, Queen Mary, University of London.
6  This file by Chris Cannam.
7 
8  This program is free software; you can redistribute it and/or
9  modify it under the terms of the GNU General Public License as
10  published by the Free Software Foundation; either version 2 of the
11  License, or (at your option) any later version. See the file
12  COPYING included with this distribution for more information.
13 */
14 
15 #include "Resampler.h"
16 
17 #include "maths/MathUtilities.h"
18 #include "base/KaiserWindow.h"
19 #include "base/SincWindow.h"
20 #include "thread/Thread.h"
21 
22 #include <iostream>
23 #include <vector>
24 #include <map>
25 #include <cassert>
26 
27 using std::vector;
28 using std::map;
29 using std::cerr;
30 using std::endl;
31 
32 //#define DEBUG_RESAMPLER 1
33 //#define DEBUG_RESAMPLER_VERBOSE 1
34 
35 Resampler::Resampler(int sourceRate, int targetRate) :
36  m_sourceRate(sourceRate),
37  m_targetRate(targetRate)
38 {
39  initialise(100, 0.02);
40 }
41 
42 Resampler::Resampler(int sourceRate, int targetRate,
43  double snr, double bandwidth) :
44  m_sourceRate(sourceRate),
45  m_targetRate(targetRate)
46 {
47  initialise(snr, bandwidth);
48 }
49 
51 {
52  delete[] m_phaseData;
53 }
54 
55 // peakToPole -> length -> beta -> window
56 static map<double, map<int, map<double, vector<double> > > >
58 
59 static Mutex
61 
62 void
63 Resampler::initialise(double snr, double bandwidth)
64 {
65  int higher = std::max(m_sourceRate, m_targetRate);
66  int lower = std::min(m_sourceRate, m_targetRate);
67 
68  m_gcd = MathUtilities::gcd(lower, higher);
69  m_peakToPole = higher / m_gcd;
70 
71  if (m_targetRate < m_sourceRate) {
72  // antialiasing filter, should be slightly below nyquist
73  m_peakToPole = m_peakToPole / (1.0 - bandwidth/2.0);
74  }
75 
77  KaiserWindow::parametersForBandwidth(snr, bandwidth, higher / m_gcd);
78 
79  params.length =
80  (params.length % 2 == 0 ? params.length + 1 : params.length);
81 
82  params.length =
83  (params.length > 200001 ? 200001 : params.length);
84 
85  m_filterLength = params.length;
86 
87  vector<double> filter;
88  knownFilterMutex.lock();
89 
90  if (knownFilters[m_peakToPole][m_filterLength].find(params.beta) ==
92 
93  KaiserWindow kw(params);
95 
96  filter = vector<double>(m_filterLength, 0.0);
97  for (int i = 0; i < m_filterLength; ++i) filter[i] = 1.0;
98  sw.cut(filter.data());
99  kw.cut(filter.data());
100 
101  knownFilters[m_peakToPole][m_filterLength][params.beta] = filter;
102  }
103 
104  filter = knownFilters[m_peakToPole][m_filterLength][params.beta];
105  knownFilterMutex.unlock();
106 
107  int inputSpacing = m_targetRate / m_gcd;
108  int outputSpacing = m_sourceRate / m_gcd;
109 
110 #ifdef DEBUG_RESAMPLER
111  cerr << "resample " << m_sourceRate << " -> " << m_targetRate
112  << ": inputSpacing " << inputSpacing << ", outputSpacing "
113  << outputSpacing << ": filter length " << m_filterLength
114  << endl;
115 #endif
116 
117  // Now we have a filter of (odd) length flen in which the lower
118  // sample rate corresponds to every n'th point and the higher rate
119  // to every m'th where n and m are higher and lower rates divided
120  // by their gcd respectively. So if x coordinates are on the same
121  // scale as our filter resolution, then source sample i is at i *
122  // (targetRate / gcd) and target sample j is at j * (sourceRate /
123  // gcd).
124 
125  // To reconstruct a single target sample, we want a buffer (real
126  // or virtual) of flen values formed of source samples spaced at
127  // intervals of (targetRate / gcd), in our example case 3. This
128  // is initially formed with the first sample at the filter peak.
129  //
130  // 0 0 0 0 a 0 0 b 0
131  //
132  // and of course we have our filter
133  //
134  // f1 f2 f3 f4 f5 f6 f7 f8 f9
135  //
136  // We take the sum of products of non-zero values from this buffer
137  // with corresponding values in the filter
138  //
139  // a * f5 + b * f8
140  //
141  // Then we drop (sourceRate / gcd) values, in our example case 4,
142  // from the start of the buffer and fill until it has flen values
143  // again
144  //
145  // a 0 0 b 0 0 c 0 0
146  //
147  // repeat to reconstruct the next target sample
148  //
149  // a * f1 + b * f4 + c * f7
150  //
151  // and so on.
152  //
153  // Above I said the buffer could be "real or virtual" -- ours is
154  // virtual. We don't actually store all the zero spacing values,
155  // except for padding at the start; normally we store only the
156  // values that actually came from the source stream, along with a
157  // phase value that tells us how many virtual zeroes there are at
158  // the start of the virtual buffer. So the two examples above are
159  //
160  // 0 a b [ with phase 1 ]
161  // a b c [ with phase 0 ]
162  //
163  // Having thus broken down the buffer so that only the elements we
164  // need to multiply are present, we can also unzip the filter into
165  // every-nth-element subsets at each phase, allowing us to do the
166  // filter multiplication as a simply vector multiply. That is, rather
167  // than store
168  //
169  // f1 f2 f3 f4 f5 f6 f7 f8 f9
170  //
171  // we store separately
172  //
173  // f1 f4 f7
174  // f2 f5 f8
175  // f3 f6 f9
176  //
177  // Each time we complete a multiply-and-sum, we need to work out
178  // how many (real) samples to drop from the start of our buffer,
179  // and how many to add at the end of it for the next multiply. We
180  // know we want to drop enough real samples to move along by one
181  // computed output sample, which is our outputSpacing number of
182  // virtual buffer samples. Depending on the relationship between
183  // input and output spacings, this may mean dropping several real
184  // samples, one real sample, or none at all (and simply moving to
185  // a different "phase").
186 
187  m_phaseData = new Phase[inputSpacing];
188 
189  for (int phase = 0; phase < inputSpacing; ++phase) {
190 
191  Phase p;
192 
193  p.nextPhase = phase - outputSpacing;
194  while (p.nextPhase < 0) p.nextPhase += inputSpacing;
195  p.nextPhase %= inputSpacing;
196 
197  p.drop = int(ceil(std::max(0.0, double(outputSpacing - phase))
198  / inputSpacing));
199 
200  int filtZipLength = int(ceil(double(m_filterLength - phase)
201  / inputSpacing));
202 
203  for (int i = 0; i < filtZipLength; ++i) {
204  p.filter.push_back(filter[i * inputSpacing + phase]);
205  }
206 
207  m_phaseData[phase] = p;
208  }
209 
210 #ifdef DEBUG_RESAMPLER
211  int cp = 0;
212  int totDrop = 0;
213  for (int i = 0; i < inputSpacing; ++i) {
214  cerr << "phase = " << cp << ", drop = " << m_phaseData[cp].drop
215  << ", filter length = " << m_phaseData[cp].filter.size()
216  << ", next phase = " << m_phaseData[cp].nextPhase << endl;
217  totDrop += m_phaseData[cp].drop;
218  cp = m_phaseData[cp].nextPhase;
219  }
220  cerr << "total drop = " << totDrop << endl;
221 #endif
222 
223  // The May implementation of this uses a pull model -- we ask the
224  // resampler for a certain number of output samples, and it asks
225  // its source stream for as many as it needs to calculate
226  // those. This means (among other things) that the source stream
227  // can be asked for enough samples up-front to fill the buffer
228  // before the first output sample is generated.
229  //
230  // In this implementation we're using a push model in which a
231  // certain number of source samples is provided and we're asked
232  // for as many output samples as that makes available. But we
233  // can't return any samples from the beginning until half the
234  // filter length has been provided as input. This means we must
235  // either return a very variable number of samples (none at all
236  // until the filter fills, then half the filter length at once) or
237  // else have a lengthy declared latency on the output. We do the
238  // latter. (What do other implementations do?)
239  //
240  // We want to make sure the first "real" sample will eventually be
241  // aligned with the centre sample in the filter (it's tidier, and
242  // easier to do diagnostic calculations that way). So we need to
243  // pick the initial phase and buffer fill accordingly.
244  //
245  // Example: if the inputSpacing is 2, outputSpacing is 3, and
246  // filter length is 7,
247  //
248  // x x x x a b c ... input samples
249  // 0 1 2 3 4 5 6 7 8 9 10 11 12 13 ...
250  // i j k l ... output samples
251  // [--------|--------] <- filter with centre mark
252  //
253  // Let h be the index of the centre mark, here 3 (generally
254  // int(filterLength/2) for odd-length filters).
255  //
256  // The smallest n such that h + n * outputSpacing > filterLength
257  // is 2 (that is, ceil((filterLength - h) / outputSpacing)), and
258  // (h + 2 * outputSpacing) % inputSpacing == 1, so the initial
259  // phase is 1.
260  //
261  // To achieve our n, we need to pre-fill the "virtual" buffer with
262  // 4 zero samples: the x's above. This is int((h + n *
263  // outputSpacing) / inputSpacing). It's the phase that makes this
264  // buffer get dealt with in such a way as to give us an effective
265  // index for sample a of 9 rather than 8 or 10 or whatever.
266  //
267  // This gives us output latency of 2 (== n), i.e. output samples i
268  // and j will appear before the one in which input sample a is at
269  // the centre of the filter.
270 
271  int h = int(m_filterLength / 2);
272  int n = ceil(double(m_filterLength - h) / outputSpacing);
273 
274  m_phase = (h + n * outputSpacing) % inputSpacing;
275 
276  int fill = (h + n * outputSpacing) / inputSpacing;
277 
278  m_latency = n;
279 
280  m_buffer = vector<double>(fill, 0);
281  m_bufferOrigin = 0;
282 
283 #ifdef DEBUG_RESAMPLER
284  cerr << "initial phase " << m_phase << " (as " << (m_filterLength/2) << " % " << inputSpacing << ")"
285  << ", latency " << m_latency << endl;
286 #endif
287 }
288 
289 double
291 {
292  Phase &pd = m_phaseData[m_phase];
293  double v = 0.0;
294  int n = pd.filter.size();
295 
296  assert(n + m_bufferOrigin <= (int)m_buffer.size());
297 
298  const double *const __restrict__ buf = m_buffer.data() + m_bufferOrigin;
299  const double *const __restrict__ filt = pd.filter.data();
300 
301  for (int i = 0; i < n; ++i) {
302  // NB gcc can only vectorize this with -ffast-math
303  v += buf[i] * filt[i];
304  }
305 
306  m_bufferOrigin += pd.drop;
307  m_phase = pd.nextPhase;
308  return v;
309 }
310 
311 int
312 Resampler::process(const double *src, double *dst, int n)
313 {
314  for (int i = 0; i < n; ++i) {
315  m_buffer.push_back(src[i]);
316  }
317 
318  int maxout = int(ceil(double(n) * m_targetRate / m_sourceRate));
319  int outidx = 0;
320 
321 #ifdef DEBUG_RESAMPLER
322  cerr << "process: buf siz " << m_buffer.size() << " filt siz for phase " << m_phase << " " << m_phaseData[m_phase].filter.size() << endl;
323 #endif
324 
325  double scaleFactor = (double(m_targetRate) / m_gcd) / m_peakToPole;
326 
327  while (outidx < maxout &&
328  m_buffer.size() >= m_phaseData[m_phase].filter.size() + m_bufferOrigin) {
329  dst[outidx] = scaleFactor * reconstructOne();
330  outidx++;
331  }
332 
333  m_buffer = vector<double>(m_buffer.begin() + m_bufferOrigin, m_buffer.end());
334  m_bufferOrigin = 0;
335 
336  return outidx;
337 }
338 
339 vector<double>
340 Resampler::process(const double *src, int n)
341 {
342  int maxout = int(ceil(double(n) * m_targetRate / m_sourceRate));
343  vector<double> out(maxout, 0.0);
344  int got = process(src, out.data(), n);
345  assert(got <= maxout);
346  if (got < maxout) out.resize(got);
347  return out;
348 }
349 
350 vector<double>
351 Resampler::resample(int sourceRate, int targetRate, const double *data, int n)
352 {
353  Resampler r(sourceRate, targetRate);
354 
355  int latency = r.getLatency();
356 
357  // latency is the output latency. We need to provide enough
358  // padding input samples at the end of input to guarantee at
359  // *least* the latency's worth of output samples. that is,
360 
361  int inputPad = int(ceil((double(latency) * sourceRate) / targetRate));
362 
363  // that means we are providing this much input in total:
364 
365  int n1 = n + inputPad;
366 
367  // and obtaining this much output in total:
368 
369  int m1 = int(ceil((double(n1) * targetRate) / sourceRate));
370 
371  // in order to return this much output to the user:
372 
373  int m = int(ceil((double(n) * targetRate) / sourceRate));
374 
375 #ifdef DEBUG_RESAMPLER
376  cerr << "n = " << n << ", sourceRate = " << sourceRate << ", targetRate = " << targetRate << ", m = " << m << ", latency = " << latency << ", inputPad = " << inputPad << ", m1 = " << m1 << ", n1 = " << n1 << ", n1 - n = " << n1 - n << endl;
377 #endif
378 
379  vector<double> pad(n1 - n, 0.0);
380  vector<double> out(m1 + 1, 0.0);
381 
382  int gotData = r.process(data, out.data(), n);
383  int gotPad = r.process(pad.data(), out.data() + gotData, pad.size());
384  int got = gotData + gotPad;
385 
386 #ifdef DEBUG_RESAMPLER
387  cerr << "resample: " << n << " in, " << pad.size() << " padding, " << got << " out (" << gotData << " data, " << gotPad << " padding, latency = " << latency << ")" << endl;
388 #endif
389 #ifdef DEBUG_RESAMPLER_VERBOSE
390  int printN = 50;
391  cerr << "first " << printN << " in:" << endl;
392  for (int i = 0; i < printN && i < n; ++i) {
393  if (i % 5 == 0) cerr << endl << i << "... ";
394  cerr << data[i] << " ";
395  }
396  cerr << endl;
397 #endif
398 
399  int toReturn = got - latency;
400  if (toReturn > m) toReturn = m;
401 
402  vector<double> sliced(out.begin() + latency,
403  out.begin() + latency + toReturn);
404 
405 #ifdef DEBUG_RESAMPLER_VERBOSE
406  cerr << "first " << printN << " out (after latency compensation), length " << sliced.size() << ":";
407  for (int i = 0; i < printN && i < sliced.size(); ++i) {
408  if (i % 5 == 0) cerr << endl << i << "... ";
409  cerr << sliced[i] << " ";
410  }
411  cerr << endl;
412 #endif
413 
414  return sliced;
415 }
416 
Kaiser window: A windower whose bandwidth and sidelobe height (signal-noise ratio) can be specified...
Definition: KaiserWindow.h:25
std::vector< double > filter
Definition: Resampler.h:88
void initialise(double, double)
Definition: Resampler.cpp:63
int m_latency
Definition: Resampler.h:83
double reconstructOne()
Definition: Resampler.cpp:290
static Mutex knownFilterMutex
Definition: Resampler.cpp:60
int m_phase
Definition: Resampler.h:93
Resampler resamples a stream from one integer sample rate to another (arbitrary) rate, using a kaiser-windowed sinc filter.
Definition: Resampler.h:30
A window containing values of the sinc function, i.e.
Definition: SincWindow.h:23
int getLatency() const
Return the number of samples of latency at the output due by the filter.
Definition: Resampler.h:68
virtual ~Resampler()
Definition: Resampler.cpp:50
int m_sourceRate
Definition: Resampler.h:78
static Parameters parametersForBandwidth(double attenuation, double bandwidth, double samplerate)
Obtain the parameters necessary for a Kaiser window of the given attenuation in dB and transition ban...
Definition: KaiserWindow.h:72
int m_filterLength
Definition: Resampler.h:81
int m_gcd
Definition: Resampler.h:80
int process(const double *src, double *dst, int n)
Read n input samples from src and write resampled data to dst.
Definition: Resampler.cpp:312
Phase * m_phaseData
Definition: Resampler.h:92
int m_bufferOrigin
Definition: Resampler.h:95
int m_targetRate
Definition: Resampler.h:79
std::vector< double > m_buffer
Definition: Resampler.h:94
Resampler(int sourceRate, int targetRate)
Construct a Resampler to resample from sourceRate to targetRate.
Definition: Resampler.cpp:35
static int gcd(int a, int b)
Return the greatest common divisor of natural numbers a and b.
static std::vector< double > resample(int sourceRate, int targetRate, const double *data, int n)
Carry out a one-off resample of a single block of n samples.
Definition: Resampler.cpp:351
void cut(double *src) const
Definition: SincWindow.h:43
double m_peakToPole
Definition: Resampler.h:84
static map< double, map< int, map< double, vector< double > > > > knownFilters
Definition: Resampler.cpp:57
void cut(double *src) const
Definition: KaiserWindow.h:87