changeset 362:3953f3ef1b62

First cut at resampler (not quite correct)

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/dsp/rateconversion/Resampler.cpp	Fri Oct 11 18:00:51 2013 +0100
@@ -0,0 +1,147 @@
+/* -*- c-basic-offset: 4 indent-tabs-mode: nil -*-  vi:set ts=8 sts=4 sw=4: */
+
+#include "Resampler.h"
+
+#include "qm-dsp/maths/MathUtilities.h"
+#include "qm-dsp/base/KaiserWindow.h"
+#include "qm-dsp/base/SincWindow.h"
+
+#include <iostream>
+
+Resampler::Resampler(int sourceRate, int targetRate) :
+    m_sourceRate(sourceRate),
+    m_targetRate(targetRate)
+{
+    initialise();
+}
+
+Resampler::~Resampler()
+{
+    delete[] m_buffer;
+    delete[] m_phaseData;
+}
+
+void
+Resampler::initialise()
+{
+    int higher = std::max(m_sourceRate, m_targetRate);
+    int lower = std::min(m_sourceRate, m_targetRate);
+
+    m_gcd = MathUtilities::gcd(lower, higher);
+
+    int peakToPole = higher / m_gcd;
+
+    KaiserWindow::Parameters params =
+	KaiserWindow::parametersForBandwidth(100, 0.02, peakToPole);
+
+    params.length =
+	(params.length % 2 == 0 ? params.length + 1 : params.length);
+    
+    m_filterLength = params.length;
+    
+    KaiserWindow kw(params);
+    SincWindow sw(m_filterLength, peakToPole * 2);
+
+    double *filter = new double[m_filterLength];
+    for (int i = 0; i < m_filterLength; ++i) filter[i] = 1.0;
+    sw.cut(filter);
+    kw.cut(filter);
+
+    int inputSpacing = m_targetRate / m_gcd;
+    int outputSpacing = m_sourceRate / m_gcd;
+
+    m_latency = int((m_filterLength / 2) / outputSpacing);
+
+    m_bufferLength = 0;
+
+    m_phaseData = new Phase[inputSpacing];
+
+    for (int phase = 0; phase < inputSpacing; ++phase) {
+
+	Phase p;
+
+	p.nextPhase = phase - outputSpacing;
+	while (p.nextPhase < 0) p.nextPhase += inputSpacing;
+	p.nextPhase %= inputSpacing;
+	
+	p.drop = int(ceil(std::max(0, outputSpacing - phase) / inputSpacing));
+	p.take = int((outputSpacing +
+		      ((m_filterLength - 1 - phase) % inputSpacing))
+		     / outputSpacing);
+
+	int filtZipLength = int(ceil((m_filterLength - phase) / inputSpacing));
+	if (filtZipLength > m_bufferLength) {
+	    m_bufferLength = filtZipLength;
+	}
+	
+	for (int i = 0; i < filtZipLength; ++i) {
+	    p.filter.push_back(filter[i * inputSpacing + phase]);
+	}
+
+	m_phaseData[phase] = p;
+    }
+
+    delete[] filter;
+
+    // The May implementation of this uses a pull model -- we ask the
+    // resampler for a certain number of output samples, and it asks
+    // its source stream for as many as it needs to calculate
+    // those. This means (among other things) that the source stream
+    // can be asked for enough samples up-front to fill the buffer
+    // before the first output sample is generated.
+    // 
+    // In this implementation we're using a push model in which a
+    // certain number of source samples is provided and we're asked
+    // for as many output samples as that makes available. But we
+    // can't return any samples from the beginning until half the
+    // filter length has been provided as input. This means we must
+    // either return a very variable number of samples (none at all
+    // until the filter fills, then half the filter length at once) or
+    // else have a lengthy declared latency on the output. We do the
+    // latter. (What do other implementations do?)
+
+    m_phase = m_filterLength % inputSpacing;
+    m_buffer = new double[m_bufferLength];
+    for (int i = 0; i < m_bufferLength; ++i) m_buffer[i] = 0.0;
+}
+
+double
+Resampler::reconstructOne(const double **srcptr)
+{
+    Phase &pd = m_phaseData[m_phase];
+    double *filt = pd.filter.data();
+    int n = pd.filter.size();
+    double v = 0.0;
+    for (int i = 0; i < n; ++i) {
+	v += m_buffer[i] * filt[i];
+    }
+    for (int i = pd.drop; i < n; ++i) {
+	m_buffer[i - pd.drop] = m_buffer[i];
+    }
+    for (int i = 0; i < pd.take; ++i) {
+	m_buffer[n - pd.drop + i] = **srcptr;
+	++ *srcptr;
+    }
+    m_phase = pd.nextPhase;
+    return v;
+}
+
+int
+Resampler::process(const double *src, double *dst, int n)
+{
+    int m = 0;
+    const double *srcptr = src;
+
+    while (n > m_phaseData[m_phase].take) {
+	std::cerr << "n = " << n << ", m = " << m << ", take = " << m_phaseData[m_phase].take << std::endl;
+	n -= m_phaseData[m_phase].take;
+	dst[m] = reconstructOne(&srcptr);
+	std::cerr << "n -> " << n << std::endl;
+	++m;
+    }
+
+    //!!! save any excess 
+
+    return m;
+}
+

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/dsp/rateconversion/Resampler.h	Fri Oct 11 18:00:51 2013 +0100
@@ -0,0 +1,58 @@
+/* -*- c-basic-offset: 4 indent-tabs-mode: nil -*-  vi:set ts=8 sts=4 sw=4: */
+
+#ifndef RESAMPLER_H
+#define RESAMPLER_H
+
+#include <vector>
+
+class Resampler
+{
+public:
+    /**
+     * Construct a Resampler to resample from sourceRate to
+     * targetRate.
+     */
+    Resampler(int sourceRate, int targetRate);
+    virtual ~Resampler();
+
+    /**
+     * Read n input samples from src and write resampled data to
+     * dst. The return value is the number of samples written, which
+     * will be no more than ceil((n * targetRate) / sourceRate). The
+     * caller must ensure the dst buffer has enough space for the
+     * samples returned.
+     */
+    int process(const double *src, double *dst, int n);
+
+    /**
+     * Return the number of samples of latency at the output due by
+     * the filter. (That is, the output will be delayed by this number
+     * of samples relative to the input.)
+     */
+    int getLatency() const { return m_latency; }
+
+private:
+    int m_sourceRate;
+    int m_targetRate;
+    int m_gcd;
+    int m_filterLength;
+    int m_bufferLength;
+    int m_latency;
+    
+    struct Phase {
+        int nextPhase;
+        std::vector<double> filter;
+        int drop;
+        int take;
+    };
+
+    Phase *m_phaseData;
+    int m_phase;
+    double *m_buffer;
+
+    void initialise();
+    double reconstructOne(const double **);
+};
+
+#endif
+    

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/dsp/rateconversion/TestResampler.cpp	Fri Oct 11 18:00:51 2013 +0100
@@ -0,0 +1,73 @@
+/* -*- c-basic-offset: 4 indent-tabs-mode: nil -*-  vi:set ts=8 sts=4 sw=4: */
+
+#include "Resampler.h"
+
+#include <iostream>
+
+#include <cmath>
+
+#define BOOST_TEST_DYN_LINK
+#define BOOST_TEST_MAIN
+
+#include <boost/test/unit_test.hpp>
+
+BOOST_AUTO_TEST_SUITE(TestResampler)
+
+using std::cout;
+using std::endl;
+
+void
+testResampler(int sourceRate,
+	      int targetRate,
+	      int n,
+	      double *in,
+	      int m,
+	      double *expected)
+{
+    Resampler r(sourceRate, targetRate);
+    int latency = r.getLatency();
+    std::cerr << "latency = " << latency << std::endl;
+
+    int m1 = m + latency;
+    int n1 = int((m1 * sourceRate) / targetRate);
+
+    double *inPadded = new double[n1];
+    double *outPadded = new double[m1];
+
+    for (int i = 0; i < n1; ++i) {
+	if (i < n) inPadded[i] = in[i];
+	else inPadded[i] = 0.0;
+    }
+    
+    for (int i = 0; i < m1; ++i) {
+	outPadded[i] = -999.0;
+    }
+
+    int got = r.process(inPadded, outPadded, n1);
+
+    std::cerr << n1 << " in, " << got << " out" << std::endl;
+
+    BOOST_CHECK_EQUAL(got, m1);
+
+    std::cerr << "results including latency padding:" << std::endl;
+    for (int i = 0; i < m1; ++i) {
+	std::cerr << outPadded[i] << " ";
+	if (i % 6 == 5) std::cerr << "\n";
+    }
+    std::cerr << "\n";
+
+    for (int i = latency; i < m1; ++i) {
+	BOOST_CHECK_CLOSE(outPadded[i], expected[i-latency], 1e-8);
+    }
+    delete[] outPadded;
+    delete[] inPadded;
+}
+
+BOOST_AUTO_TEST_CASE(sameRate)
+{
+    double d[] = { 0, 0.1, -0.3, -0.4, -0.3, 0, 0.5, 0.2, 0.8, -0.1 };
+    testResampler(4, 4, 10, d, 10, d);
+}
+
+BOOST_AUTO_TEST_SUITE_END()
+