qm-dsp
1.8
|
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