diff.h

#ifndef _DIFF_H_
#define _DIFF_H_

#include <cassert>
#include <iostream>
#include <vector>
#include "debug.h"


typedef std::pair <float,float> Maximum; // Point stores position and value at maxima

// Calculate difference of elements in vector (aproximate derivative).
// Vector must not be empty and output-vector has one less element.
template <typename T>
std::vector<T> diff(std::vector<T> const& input) {
  assert(input.size());

  std::vector<T> output(input.size() - 1);
  for(unsigned i = 0; i < output.size(); ++i) {
    output[i] = input[i + 1] - input[i];
  }

  return output;
}

template <typename T>
T greatest(std::vector<T> const& xx) {
  assert(xx.size() >= 1);

  T m = xx[0];

  for(unsigned i = 1; i < xx.size(); ++i) {
    if(xx[i] > m) m = xx[i];
  }

  return m;
}
template <typename T>

T smallest(std::vector<T> const& xx) {
  assert(xx.size() >= 1);

  T m = xx[0];

  for(unsigned i = 1; i < xx.size(); ++i) {
    if(xx[i] < m) m = xx[i];
  }

  return m;
}

template <typename T>
double linear_interpolation(double subpos, T const& a, T const& b) {
  return a + subpos * (b - a);
}

inline double interpolate_phase(double freq, std::vector<double> const& phase) {
  unsigned floor = unsigned(freq);
  unsigned ceiling = unsigned(freq + 0.999999);
  double subpos = freq - (double)floor;
  //cout << "freq = " << freq << ", subpos = " << subpos << endl;
  //cout << "floor = " << floor << ", ceiling = " << ceiling << endl;
  //cout << "[floor] = " << phase[floor] << ", [ceiling] = " << phase[ceiling] << endl;

  double y = linear_interpolation(subpos, phase[floor], phase[ceiling]);
  //cout << "estimate = " << y << endl;
  return y;
}

// Find local maxima in xx-vector. The return-vector has sub-index
// precision estimating from by polynomial approximation of
// derivative. The interval [-delta;+delta] is considered zero.  Only
// extrema that are greater than global_maxima * relative_threshold
// are returned.

template <typename T>
std::vector<Maximum> local_maxima(std::vector<T> const& xx, double delta, double relative_threshold = 0.0) {
  std::vector<Maximum> maxima;

  // Perhaps threshold in frequency range should be allowed
  // Perhaps the magnitude frequency should be independent of data

  T threshold = relative_threshold * greatest(xx);

  for(unsigned i = 1; i < xx.size() - 1; ++i) {
    // Look for cross-overs where xx[i-1] <= xx[i] >= xx[i+1]
    if(xx[i] >= threshold && xx[i-1] + delta <= xx[i] && xx[i] - delta >= xx[i+1]) {
      // We approximate the signal using a polynomial defined by
      // points xx[i-1], xx[i] & xx[i+1]. 
      // xx[i-1] = A*(-1)^2 + B*(-1) + C = A - B + C
      // xx[i]   = A*(0)^2 + B*(0) + C = C
      // xx[i+1] = A*(1)^2 + B*(1) + C = A + B + C
      // 
      // This requires that: xx[i+1]=A+B+C  =>  xx[i+1]-B=A+C
      //                and: xx[i-1]=A-B+C  =>  xx[i-1]+B=A+C
      // The lines combine to make:
      //  xx[i+1]-B = xx[i-1]+B  =>  xx[i+1]-xx[i-1] = 2B
      //                         =>  B = 0.5*(xx[i+1]-xx[i-1])
      // 
      // xx[i+1] = A + B + C  =>  A = xx[i+1] - B - C

      //dbg2("i = %d\n", i);
      //dbg4("left=%f, center=%f, right=%f\n", xx[i-1], xx[i], xx[i+1]);

      double C = xx[i];
      double B = 0.5 * (xx[i+1] - xx[i-1]);
      double A = xx[i+1] - B - C;
      //dbg4("A=%f, B=%f, C=%f\n", A, B, C);

      // To find exact maximum the polynomial is differentiated
      // to get Y = 2AX + B which is zero in X = -B/2A
      double sub_idx = -0.5 * B / A;      
      //dbg2("sub-idx = %f\n", sub_idx);
      double value = A*sub_idx*sub_idx + B*sub_idx + C;
      //dbg2("value = %f\n", value);
      maxima.push_back(Maximum(sub_idx + i, value));
    }
  }
  return maxima;
}

#endif//_DIFF_H_

---