libs/qm-dsp/maths/MedianFilter.h

   1 /* -*- c-basic-offset: 4 indent-tabs-mode: nil -*-  vi:set ts=8 sts=4 sw=4: */
   2
   3 /*
   4     QM DSP Library
   5
   6     Centre for Digital Music, Queen Mary, University of London.
   7     This file Copyright 2010 Chris Cannam.
   8
   9     This program is free software; you can redistribute it and/or
  10     modify it under the terms of the GNU General Public License as
  11     published by the Free Software Foundation; either version 2 of the
  12     License, or (at your option) any later version.  See the file
  13     COPYING included with this distribution for more information.
  14 */
  15
  16 #ifndef MEDIAN_FILTER_H
  17 #define MEDIAN_FILTER_H
  18
  19 #include <algorithm>
  20 #include <cassert>
  21 #include <cmath>
  22 #include <iostream>
  23 #include <vector>
  24
  25 template <typename T>
  26 class MedianFilter
  27 {
  28 public:
  29     MedianFilter(int size, float percentile = 50.f) :
  30         m_size(size),
  31         m_frame(new T[size]),
  32         m_sorted(new T[size]),
  33         m_sortend(m_sorted + size - 1) {
  34         setPercentile(percentile);
  35         reset();
  36     }
  37
  38     ~MedianFilter() {
  39         delete[] m_frame;
  40         delete[] m_sorted;
  41     }
  42
  43     void setPercentile(float p) {
  44         m_index = int((m_size * p) / 100.f);
  45         if (m_index >= m_size) m_index = m_size-1;
  46         if (m_index < 0) m_index = 0;
  47     }
  48
  49     void push(T value) {
  50         if (value != value) {
  51             std::cerr << "WARNING: MedianFilter::push: attempt to push NaN, pushing zero instead" << std::endl;
  52             // we do need to push something, to maintain the filter length
  53             value = T();
  54         }
  55         drop(m_frame[0]);
  56         const int sz1 = m_size-1;
  57         for (int i = 0; i < sz1; ++i) m_frame[i] = m_frame[i+1];
  58         m_frame[m_size-1] = value;
  59         put(value);
  60     }
  61
  62     T get() const {
  63         return m_sorted[m_index];
  64     }
  65
  66     int getSize() const {
  67         return m_size;
  68     }
  69
  70     T getAt(float percentile) {
  71         int ix = int((m_size * percentile) / 100.f);
  72         if (ix >= m_size) ix = m_size-1;
  73         if (ix < 0) ix = 0;
  74         return m_sorted[ix];
  75     }
  76
  77     void reset() {
  78         for (int i = 0; i < m_size; ++i) m_frame[i] = 0;
  79         for (int i = 0; i < m_size; ++i) m_sorted[i] = 0;
  80     }
  81
  82     static std::vector<T> filter(int size, const std::vector<T> &in) {
  83         std::vector<T> out;
  84         MedianFilter<T> f(size);
  85         for (int i = 0; i < int(in.size()); ++i) {
  86             f.push(in[i]);
  87             T median = f.get();
  88             if (i >= size/2) out.push_back(median);
  89         }
  90         while (out.size() < in.size()) {
  91             f.push(T());
  92             out.push_back(f.get());
  93         }
  94         return out;
  95     }
  96
  97 private:
  98     const int m_size;
  99     T *const m_frame;
 100     T *const m_sorted;
 101     T *const m_sortend;
 102     int m_index;
 103
 104     void put(T value) {
 105         // precondition: m_sorted contains m_size-1 values, packed at start
 106         // postcondition: m_sorted contains m_size values, one of which is value
 107         T *point = std::lower_bound(m_sorted, m_sortend, value);
 108         const int n = m_sortend - point;
 109         for (int i = n; i > 0; --i) point[i] = point[i-1];
 110         *point = value;
 111     }
 112
 113     void drop(T value) {
 114         // precondition: m_sorted contains m_size values, one of which is value
 115         // postcondition: m_sorted contains m_size-1 values, packed at start
 116         T *point = std::lower_bound(m_sorted, m_sortend + 1, value);
 117         if (*point != value) {
 118             std::cerr << "WARNING: MedianFilter::drop: *point is " << *point
 119                       << ", expected " << value << std::endl;
 120         }
 121         const int n = m_sortend - point;
 122         for (int i = 0; i < n; ++i) point[i] = point[i+1];
 123         *m_sortend = T(0);
 124     }
 125
 126     MedianFilter(const MedianFilter &); // not provided
 127     MedianFilter &operator=(const MedianFilter &); // not provided
 128 };
 129
 130 #endif
 131