qm-dsp  1.8
Resampler.cpp
Go to the documentation of this file.
00001 /* -*- c-basic-offset: 4 indent-tabs-mode: nil -*-  vi:set ts=8 sts=4 sw=4: */
00002 /*
00003     QM DSP Library
00004 
00005     Centre for Digital Music, Queen Mary, University of London.
00006     This file by Chris Cannam.
00007 
00008     This program is free software; you can redistribute it and/or
00009     modify it under the terms of the GNU General Public License as
00010     published by the Free Software Foundation; either version 2 of the
00011     License, or (at your option) any later version.  See the file
00012     COPYING included with this distribution for more information.
00013 */
00014 
00015 #include "Resampler.h"
00016 
00017 #include "maths/MathUtilities.h"
00018 #include "base/KaiserWindow.h"
00019 #include "base/SincWindow.h"
00020 #include "thread/Thread.h"
00021 
00022 #include <iostream>
00023 #include <vector>
00024 #include <map>
00025 #include <cassert>
00026 
00027 using std::vector;
00028 using std::map;
00029 using std::cerr;
00030 using std::endl;
00031 
00032 //#define DEBUG_RESAMPLER 1
00033 //#define DEBUG_RESAMPLER_VERBOSE 1
00034 
00035 Resampler::Resampler(int sourceRate, int targetRate) :
00036     m_sourceRate(sourceRate),
00037     m_targetRate(targetRate)
00038 {
00039     initialise(100, 0.02);
00040 }
00041 
00042 Resampler::Resampler(int sourceRate, int targetRate, 
00043                      double snr, double bandwidth) :
00044     m_sourceRate(sourceRate),
00045     m_targetRate(targetRate)
00046 {
00047     initialise(snr, bandwidth);
00048 }
00049 
00050 Resampler::~Resampler()
00051 {
00052     delete[] m_phaseData;
00053 }
00054 
00055 // peakToPole -> length -> beta -> window
00056 static map<double, map<int, map<double, vector<double> > > >
00057 knownFilters;
00058 
00059 static Mutex
00060 knownFilterMutex;
00061 
00062 void
00063 Resampler::initialise(double snr, double bandwidth)
00064 {
00065     int higher = std::max(m_sourceRate, m_targetRate);
00066     int lower = std::min(m_sourceRate, m_targetRate);
00067 
00068     m_gcd = MathUtilities::gcd(lower, higher);
00069     m_peakToPole = higher / m_gcd;
00070 
00071     if (m_targetRate < m_sourceRate) {
00072         // antialiasing filter, should be slightly below nyquist
00073         m_peakToPole = m_peakToPole / (1.0 - bandwidth/2.0);
00074     }
00075 
00076     KaiserWindow::Parameters params =
00077         KaiserWindow::parametersForBandwidth(snr, bandwidth, higher / m_gcd);
00078 
00079     params.length =
00080         (params.length % 2 == 0 ? params.length + 1 : params.length);
00081     
00082     params.length =
00083         (params.length > 200001 ? 200001 : params.length);
00084 
00085     m_filterLength = params.length;
00086 
00087     vector<double> filter;
00088     knownFilterMutex.lock();
00089 
00090     if (knownFilters[m_peakToPole][m_filterLength].find(params.beta) ==
00091         knownFilters[m_peakToPole][m_filterLength].end()) {
00092 
00093         KaiserWindow kw(params);
00094         SincWindow sw(m_filterLength, m_peakToPole * 2);
00095 
00096         filter = vector<double>(m_filterLength, 0.0);
00097         for (int i = 0; i < m_filterLength; ++i) filter[i] = 1.0;
00098         sw.cut(filter.data());
00099         kw.cut(filter.data());
00100 
00101         knownFilters[m_peakToPole][m_filterLength][params.beta] = filter;
00102     }
00103 
00104     filter = knownFilters[m_peakToPole][m_filterLength][params.beta];
00105     knownFilterMutex.unlock();
00106 
00107     int inputSpacing = m_targetRate / m_gcd;
00108     int outputSpacing = m_sourceRate / m_gcd;
00109 
00110 #ifdef DEBUG_RESAMPLER
00111     cerr << "resample " << m_sourceRate << " -> " << m_targetRate
00112          << ": inputSpacing " << inputSpacing << ", outputSpacing "
00113          << outputSpacing << ": filter length " << m_filterLength
00114          << endl;
00115 #endif
00116 
00117     // Now we have a filter of (odd) length flen in which the lower
00118     // sample rate corresponds to every n'th point and the higher rate
00119     // to every m'th where n and m are higher and lower rates divided
00120     // by their gcd respectively. So if x coordinates are on the same
00121     // scale as our filter resolution, then source sample i is at i *
00122     // (targetRate / gcd) and target sample j is at j * (sourceRate /
00123     // gcd).
00124 
00125     // To reconstruct a single target sample, we want a buffer (real
00126     // or virtual) of flen values formed of source samples spaced at
00127     // intervals of (targetRate / gcd), in our example case 3.  This
00128     // is initially formed with the first sample at the filter peak.
00129     //
00130     // 0  0  0  0  a  0  0  b  0
00131     //
00132     // and of course we have our filter
00133     //
00134     // f1 f2 f3 f4 f5 f6 f7 f8 f9
00135     //
00136     // We take the sum of products of non-zero values from this buffer
00137     // with corresponding values in the filter
00138     //
00139     // a * f5 + b * f8
00140     //
00141     // Then we drop (sourceRate / gcd) values, in our example case 4,
00142     // from the start of the buffer and fill until it has flen values
00143     // again
00144     //
00145     // a  0  0  b  0  0  c  0  0
00146     //
00147     // repeat to reconstruct the next target sample
00148     //
00149     // a * f1 + b * f4 + c * f7
00150     //
00151     // and so on.
00152     //
00153     // Above I said the buffer could be "real or virtual" -- ours is
00154     // virtual. We don't actually store all the zero spacing values,
00155     // except for padding at the start; normally we store only the
00156     // values that actually came from the source stream, along with a
00157     // phase value that tells us how many virtual zeroes there are at
00158     // the start of the virtual buffer.  So the two examples above are
00159     //
00160     // 0 a b  [ with phase 1 ]
00161     // a b c  [ with phase 0 ]
00162     //
00163     // Having thus broken down the buffer so that only the elements we
00164     // need to multiply are present, we can also unzip the filter into
00165     // every-nth-element subsets at each phase, allowing us to do the
00166     // filter multiplication as a simply vector multiply. That is, rather
00167     // than store
00168     //
00169     // f1 f2 f3 f4 f5 f6 f7 f8 f9
00170     // 
00171     // we store separately
00172     //
00173     // f1 f4 f7
00174     // f2 f5 f8
00175     // f3 f6 f9
00176     //
00177     // Each time we complete a multiply-and-sum, we need to work out
00178     // how many (real) samples to drop from the start of our buffer,
00179     // and how many to add at the end of it for the next multiply.  We
00180     // know we want to drop enough real samples to move along by one
00181     // computed output sample, which is our outputSpacing number of
00182     // virtual buffer samples. Depending on the relationship between
00183     // input and output spacings, this may mean dropping several real
00184     // samples, one real sample, or none at all (and simply moving to
00185     // a different "phase").
00186 
00187     m_phaseData = new Phase[inputSpacing];
00188 
00189     for (int phase = 0; phase < inputSpacing; ++phase) {
00190 
00191         Phase p;
00192 
00193         p.nextPhase = phase - outputSpacing;
00194         while (p.nextPhase < 0) p.nextPhase += inputSpacing;
00195         p.nextPhase %= inputSpacing;
00196         
00197         p.drop = int(ceil(std::max(0.0, double(outputSpacing - phase))
00198                           / inputSpacing));
00199 
00200         int filtZipLength = int(ceil(double(m_filterLength - phase)
00201                                      / inputSpacing));
00202 
00203         for (int i = 0; i < filtZipLength; ++i) {
00204             p.filter.push_back(filter[i * inputSpacing + phase]);
00205         }
00206 
00207         m_phaseData[phase] = p;
00208     }
00209 
00210 #ifdef DEBUG_RESAMPLER
00211     int cp = 0;
00212     int totDrop = 0;
00213     for (int i = 0; i < inputSpacing; ++i) {
00214         cerr << "phase = " << cp << ", drop = " << m_phaseData[cp].drop
00215              << ", filter length = " << m_phaseData[cp].filter.size()
00216              << ", next phase = " << m_phaseData[cp].nextPhase << endl;
00217         totDrop += m_phaseData[cp].drop;
00218         cp = m_phaseData[cp].nextPhase;
00219     }
00220     cerr << "total drop = " << totDrop << endl;
00221 #endif
00222 
00223     // The May implementation of this uses a pull model -- we ask the
00224     // resampler for a certain number of output samples, and it asks
00225     // its source stream for as many as it needs to calculate
00226     // those. This means (among other things) that the source stream
00227     // can be asked for enough samples up-front to fill the buffer
00228     // before the first output sample is generated.
00229     // 
00230     // In this implementation we're using a push model in which a
00231     // certain number of source samples is provided and we're asked
00232     // for as many output samples as that makes available. But we
00233     // can't return any samples from the beginning until half the
00234     // filter length has been provided as input. This means we must
00235     // either return a very variable number of samples (none at all
00236     // until the filter fills, then half the filter length at once) or
00237     // else have a lengthy declared latency on the output. We do the
00238     // latter. (What do other implementations do?)
00239     //
00240     // We want to make sure the first "real" sample will eventually be
00241     // aligned with the centre sample in the filter (it's tidier, and
00242     // easier to do diagnostic calculations that way). So we need to
00243     // pick the initial phase and buffer fill accordingly.
00244     // 
00245     // Example: if the inputSpacing is 2, outputSpacing is 3, and
00246     // filter length is 7,
00247     // 
00248     //    x     x     x     x     a     b     c ... input samples
00249     // 0  1  2  3  4  5  6  7  8  9 10 11 12 13 ... 
00250     //          i        j        k        l    ... output samples
00251     // [--------|--------] <- filter with centre mark
00252     //
00253     // Let h be the index of the centre mark, here 3 (generally
00254     // int(filterLength/2) for odd-length filters).
00255     //
00256     // The smallest n such that h + n * outputSpacing > filterLength
00257     // is 2 (that is, ceil((filterLength - h) / outputSpacing)), and
00258     // (h + 2 * outputSpacing) % inputSpacing == 1, so the initial
00259     // phase is 1.
00260     //
00261     // To achieve our n, we need to pre-fill the "virtual" buffer with
00262     // 4 zero samples: the x's above. This is int((h + n *
00263     // outputSpacing) / inputSpacing). It's the phase that makes this
00264     // buffer get dealt with in such a way as to give us an effective
00265     // index for sample a of 9 rather than 8 or 10 or whatever.
00266     //
00267     // This gives us output latency of 2 (== n), i.e. output samples i
00268     // and j will appear before the one in which input sample a is at
00269     // the centre of the filter.
00270 
00271     int h = int(m_filterLength / 2);
00272     int n = ceil(double(m_filterLength - h) / outputSpacing);
00273     
00274     m_phase = (h + n * outputSpacing) % inputSpacing;
00275 
00276     int fill = (h + n * outputSpacing) / inputSpacing;
00277     
00278     m_latency = n;
00279 
00280     m_buffer = vector<double>(fill, 0);
00281     m_bufferOrigin = 0;
00282 
00283 #ifdef DEBUG_RESAMPLER
00284     cerr << "initial phase " << m_phase << " (as " << (m_filterLength/2) << " % " << inputSpacing << ")"
00285               << ", latency " << m_latency << endl;
00286 #endif
00287 }
00288 
00289 double
00290 Resampler::reconstructOne()
00291 {
00292     Phase &pd = m_phaseData[m_phase];
00293     double v = 0.0;
00294     int n = pd.filter.size();
00295 
00296     assert(n + m_bufferOrigin <= (int)m_buffer.size());
00297 
00298     const double *const __restrict__ buf = m_buffer.data() + m_bufferOrigin;
00299     const double *const __restrict__ filt = pd.filter.data();
00300 
00301     for (int i = 0; i < n; ++i) {
00302         // NB gcc can only vectorize this with -ffast-math
00303         v += buf[i] * filt[i];
00304     }
00305 
00306     m_bufferOrigin += pd.drop;
00307     m_phase = pd.nextPhase;
00308     return v;
00309 }
00310 
00311 int
00312 Resampler::process(const double *src, double *dst, int n)
00313 {
00314     for (int i = 0; i < n; ++i) {
00315         m_buffer.push_back(src[i]);
00316     }
00317 
00318     int maxout = int(ceil(double(n) * m_targetRate / m_sourceRate));
00319     int outidx = 0;
00320 
00321 #ifdef DEBUG_RESAMPLER
00322     cerr << "process: buf siz " << m_buffer.size() << " filt siz for phase " << m_phase << " " << m_phaseData[m_phase].filter.size() << endl;
00323 #endif
00324 
00325     double scaleFactor = (double(m_targetRate) / m_gcd) / m_peakToPole;
00326 
00327     while (outidx < maxout &&
00328            m_buffer.size() >= m_phaseData[m_phase].filter.size() + m_bufferOrigin) {
00329         dst[outidx] = scaleFactor * reconstructOne();
00330         outidx++;
00331     }
00332 
00333     m_buffer = vector<double>(m_buffer.begin() + m_bufferOrigin, m_buffer.end());
00334     m_bufferOrigin = 0;
00335     
00336     return outidx;
00337 }
00338     
00339 vector<double>
00340 Resampler::process(const double *src, int n)
00341 {
00342     int maxout = int(ceil(double(n) * m_targetRate / m_sourceRate));
00343     vector<double> out(maxout, 0.0);
00344     int got = process(src, out.data(), n);
00345     assert(got <= maxout);
00346     if (got < maxout) out.resize(got);
00347     return out;
00348 }
00349 
00350 vector<double>
00351 Resampler::resample(int sourceRate, int targetRate, const double *data, int n)
00352 {
00353     Resampler r(sourceRate, targetRate);
00354 
00355     int latency = r.getLatency();
00356 
00357     // latency is the output latency. We need to provide enough
00358     // padding input samples at the end of input to guarantee at
00359     // *least* the latency's worth of output samples. that is,
00360 
00361     int inputPad = int(ceil((double(latency) * sourceRate) / targetRate));
00362 
00363     // that means we are providing this much input in total:
00364 
00365     int n1 = n + inputPad;
00366 
00367     // and obtaining this much output in total:
00368 
00369     int m1 = int(ceil((double(n1) * targetRate) / sourceRate));
00370 
00371     // in order to return this much output to the user:
00372 
00373     int m = int(ceil((double(n) * targetRate) / sourceRate));
00374     
00375 #ifdef DEBUG_RESAMPLER
00376     cerr << "n = " << n << ", sourceRate = " << sourceRate << ", targetRate = " << targetRate << ", m = " << m << ", latency = " << latency << ", inputPad = " << inputPad << ", m1 = " << m1 << ", n1 = " << n1 << ", n1 - n = " << n1 - n << endl;
00377 #endif
00378 
00379     vector<double> pad(n1 - n, 0.0);
00380     vector<double> out(m1 + 1, 0.0);
00381 
00382     int gotData = r.process(data, out.data(), n);
00383     int gotPad = r.process(pad.data(), out.data() + gotData, pad.size());
00384     int got = gotData + gotPad;
00385     
00386 #ifdef DEBUG_RESAMPLER
00387     cerr << "resample: " << n << " in, " << pad.size() << " padding, " << got << " out (" << gotData << " data, " << gotPad << " padding, latency = " << latency << ")" << endl;
00388 #endif
00389 #ifdef DEBUG_RESAMPLER_VERBOSE
00390     int printN = 50;
00391     cerr << "first " << printN << " in:" << endl;
00392     for (int i = 0; i < printN && i < n; ++i) {
00393         if (i % 5 == 0) cerr << endl << i << "... ";
00394         cerr << data[i] << " ";
00395     }
00396     cerr << endl;
00397 #endif
00398 
00399     int toReturn = got - latency;
00400     if (toReturn > m) toReturn = m;
00401 
00402     vector<double> sliced(out.begin() + latency, 
00403                           out.begin() + latency + toReturn);
00404 
00405 #ifdef DEBUG_RESAMPLER_VERBOSE
00406     cerr << "first " << printN << " out (after latency compensation), length " << sliced.size() << ":";
00407     for (int i = 0; i < printN && i < sliced.size(); ++i) {
00408         if (i % 5 == 0) cerr << endl << i << "... ";
00409         cerr << sliced[i] << " ";
00410     }
00411     cerr << endl;
00412 #endif
00413 
00414     return sliced;
00415 }
00416