source: Sophya/trunk/ArchTOIPipe/Processors/toimedfilter.cc

// ArchTOIPipe           (C)     CEA/DAPNIA/SPP IN2P3/LAL
//                               Eric Aubourg
//                               Christophe Magneville
//                               Reza Ansari
// $Id: toimedfilter.cc,v 1.4 2002-11-28 15:49:21 aubourg Exp $

#include <algorithm>
#include <set>
#include "toimedfilter.h"

TOIMedFilter::TOIMedFilter(int width)
  : w((width/2)*2+1)
{}

void TOIMedFilter::init() {
  declareInput("signal");
  declareOutput("out");
  name="medianfilter";
  upExtra = w/2;
  lowExtra = w/2;
  setNeededHistory(w+1);
}

void TOIMedFilter::run_raw() {
  int snb = getMinOut()-w/2;
  int sne = getMaxOut()+w/2;

  const int window = (w>200) ? 5*w : 1000;
  double* data = new double[window];
  double* filter = new double[w];

  for (int i=snb; i<=sne; i += window-w+1) {
    int j = i+window-1;
    if (j>=sne) j = sne;
    //   cout << "median: fetching " << i << " -> " << j << endl;
    getData(0, i, j-i+1, data);
    for (int ii=i+w/2; ii<=j-w/2; ii++) {
      //      cout << "median: computing " << ii << endl;
      copy(data+(ii-i-w/2), data+(ii-i+w/2)+1, filter);
      sort(filter, filter+w);
      //      if (ii == snb+w/2 || (ii%100 == 0))
      //        cout << ii << " -> " << filter[w/2] << endl;
      putData(0, ii, filter[w/2]);
    }
  }

  delete[] filter;
  delete[] data;
}

void TOIMedFilter::run() {
  cout << "TOIMedFilter in : " << getMinIn() << " - " << getMaxIn() 
       << " out : " << getMinOut() << " - " << getMaxOut() << endl;
  multiset<double> low;  // w/2
  multiset<double> high; // w/2
  double mid;

  int snb = getMinOut()-w/2;
  int sne = getMaxOut()+w/2;

  // init structures
  {for (int i=0; i<w; i++) {
    double data = getData(0, snb+i);
    low.insert(data);
  }}

  multiset<double>::iterator ii = low.begin();
  {for (int i=0; i<w/2; i++, ii++);}
  mid = *ii;
  ii++; high.insert(ii, low.end());
  ii--; low.erase(ii, low.end());

  putData(0, snb+w/2, mid);
    cout << snb+w/2<< " -> " << mid << endl;

  {for (int i=snb+w; i<=sne; i ++) {
    double b = getData(0, i);
    double a = getData(0, i-w);
    if (a < mid && b <= mid) {
      low.erase(low.find(a));
      low.insert(b);
    } else if (a > mid && b >= mid) {
      high.erase(high.find(a));
      high.insert(b);
    } else if (a == mid) {
      if ( b <= *high.begin() && b >= *low.rbegin()) {
        mid = b;
      } else if (b > *high.begin()) {
        mid = *high.begin();
        high.erase(high.begin());
        high.insert(b);
      } else {
        mid = *low.rbegin();
        multiset<double>::iterator i = low.end(); i--;
        low.erase(i);
        low.insert(b);
      }
    } else if (a < mid && b > mid) {
      low.erase(low.find(a));
      low.insert(mid);
      mid = *high.begin();
      high.erase(high.begin());
      high.insert(b);
    } else { // a >mid && b < mid
      if (! ( a>mid && b<mid)) {
        cout << "assertion failed " << b << " " << mid << " " << a << endl;
        abort();
      }
      high.erase(high.find(a));
      high.insert(mid);
      mid = *low.rbegin();
      multiset<double>::iterator i = low.end(); i--;
      low.erase(i);
      low.insert(b);
    }
    if (low.size() != w/2 || high.size() != w/2) {
      cout << "assertion failed " << low.size() << " " << high.size() << endl;
      abort();
    }
    //    if ( (i-w/2)%100 == 0) 
    //      cout << i-w/2 << " -> " << mid << endl;
    putData(0, i-w/2, mid);      
  }}
}