savgolfilter.cpp

Go to the documentation of this file.

/* BEGIN software license
 *
 * msXpertSuite - mass spectrometry software suite
 * -----------------------------------------------
 * Copyright(C) 2009,...,2018 Filippo Rusconi
 *
 * http://www.msxpertsuite.org
 *
 * This file is part of the msXpertSuite project.
 *
 * The msXpertSuite project is the successor of the massXpert project. This
 * project now includes various independent modules:
 *
 * - massXpert, model polymer chemistries and simulate mass spectrometric data;
 * - mineXpert, a powerful TIC chromatogram/mass spectrum viewer/miner;
 *
 * This program is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program. If not, see <http://www.gnu.org/licenses/>.
 *
 * END software license
 */
 
 
#include <qmath.h>
 
 
#include <QVector>
#include <QDebug>
 
#include "pappsomspp/core/exception/exceptionnotrecognized.h"
 
#include "savgolfilter.h"
 
 
namespace pappso
{
 
 
#define SWAP(a, b) \
  tempr = (a);     \
  (a)   = (b);     \
  (b)   = tempr
 
FilterSavitzkyGolay::FilterSavitzkyGolay()
{
  // m_params will have defaults that work well with mass spectra.
}
 
FilterSavitzkyGolay::FilterSavitzkyGolay(
  int nL, int nR, int m, int lD, bool convolveWithNr)
{
  m_params.nL             = nL;
  m_params.nR             = nR;
  m_params.m              = m;
  m_params.lD             = lD;
  m_params.convolveWithNr = convolveWithNr;
}
 
 
FilterSavitzkyGolay::FilterSavitzkyGolay(const SavGolParams sav_gol_params)
{
  m_params.nL             = sav_gol_params.nL;
  m_params.nR             = sav_gol_params.nR;
  m_params.m              = sav_gol_params.m;
  m_params.lD             = sav_gol_params.lD;
  m_params.convolveWithNr = sav_gol_params.convolveWithNr;
}
 
 
FilterSavitzkyGolay::FilterSavitzkyGolay(const QString &parameters)
{
  buildFilterFromString(parameters);
}
 
 
FilterSavitzkyGolay::FilterSavitzkyGolay(const FilterSavitzkyGolay &other)
{
  // This function only copies the parameters, not the data.
 
  m_params.nL             = other.m_params.nL;
  m_params.nR             = other.m_params.nR;
  m_params.m              = other.m_params.m;
  m_params.lD             = other.m_params.lD;
  m_params.convolveWithNr = other.m_params.convolveWithNr;
}
 
 
FilterSavitzkyGolay::~FilterSavitzkyGolay()
{
}
 
FilterSavitzkyGolay &
FilterSavitzkyGolay::operator=(const FilterSavitzkyGolay &other)
{
  if(&other == this)
    return *this;
 
  // This function only copies the parameters, not the data.
 
  m_params.nL             = other.m_params.nL;
  m_params.nR             = other.m_params.nR;
  m_params.m              = other.m_params.m;
  m_params.lD             = other.m_params.lD;
  m_params.convolveWithNr = other.m_params.convolveWithNr;
 
  return *this;
}
 
 
void
FilterSavitzkyGolay::buildFilterFromString(const QString &parameters)
{
  // Typical string: "Savitzky-Golay|15;15;4;0;false"
  if(parameters.startsWith(QString("%1|").arg(name())))
    {
      QStringList params = parameters.split("|").back().split(";");
 
      m_params.nL             = params.at(0).toInt();
      m_params.nR             = params.at(1).toInt();
      m_params.m              = params.at(2).toInt();
      m_params.lD             = params.at(3).toInt();
      m_params.convolveWithNr = (params.at(4) == "true" ? true : false);
    }
  else
    {
      throw pappso::ExceptionNotRecognized(
        QString("Building of FilterSavitzkyGolay from string %1 failed")
          .arg(parameters));
    }
}
 
 
Trace &
FilterSavitzkyGolay::filter(Trace &data_points) const
{
  // Initialize data:
 
  // We want the filter to stay constant so we create a local copy of the data.
 
  int data_point_count = data_points.size();
 
  pappso_double *x_data_p          = dvector(1, data_point_count);
  pappso_double *y_initial_data_p  = dvector(1, data_point_count);
  pappso_double *y_filtered_data_p = nullptr;
 
  if(m_params.convolveWithNr)
    y_filtered_data_p = dvector(1, 2 * data_point_count);
  else
    y_filtered_data_p = dvector(1, data_point_count);
 
  for(int iter = 0; iter < data_point_count; ++iter)
    {
      x_data_p[iter]         = data_points.at(iter).x;
      y_initial_data_p[iter] = data_points.at(iter).y;
    }
 
  // Now run the filter.
 
  runFilter(y_initial_data_p, y_filtered_data_p, data_point_count);
 
  // Put back the modified y values into the trace.
  auto iter_yf = y_filtered_data_p;
  for(auto &data_point : data_points)
    {
      data_point.y = *iter_yf;
      iter_yf++;
    }
 
  return data_points;
}
 
 
SavGolParams
FilterSavitzkyGolay::getParameters() const
{
  return SavGolParams(
    m_params.nL, m_params.nR, m_params.m, m_params.lD, m_params.convolveWithNr);
}
 
 
//! Return a string with the textual representation of the configuration data.
QString
FilterSavitzkyGolay::toString() const
{
  return QString("%1|%2").arg(name()).arg(m_params.toString());
}
 
 
QString
FilterSavitzkyGolay::name() const
{
  return "Savitzky-Golay";
}
 
 
int *
FilterSavitzkyGolay::ivector(long nl, long nh) const
{
  int *v;
  v = (int *)malloc((size_t)((nh - nl + 2) * sizeof(int)));
  if(!v)
    {
      qFatal("Error: Allocation failure.");
    }
  return v - nl + 1;
}
 
 
pappso_double *
FilterSavitzkyGolay::dvector(long nl, long nh) const
{
  pappso_double *v;
  long k;
  v = (pappso_double *)malloc((size_t)((nh - nl + 2) * sizeof(pappso_double)));
  if(!v)
    {
      qFatal("Error: Allocation failure.");
    }
  for(k = nl; k <= nh; k++)
    v[k] = 0.0;
  return v - nl + 1;
}
 
 
pappso_double **
FilterSavitzkyGolay::dmatrix(long nrl, long nrh, long ncl, long nch) const
{
  long i, nrow = nrh - nrl + 1, ncol = nch - ncl + 1;
  pappso_double **m;
  m = (pappso_double **)malloc((size_t)((nrow + 1) * sizeof(pappso_double *)));
  if(!m)
    {
      qFatal("Error: Allocation failure.");
    }
  m += 1;
  m -= nrl;
  m[nrl] = (pappso_double *)malloc(
    (size_t)((nrow * ncol + 1) * sizeof(pappso_double)));
  if(!m[nrl])
    {
      qFatal("Error: Allocation failure.");
    }
  m[nrl] += 1;
  m[nrl] -= ncl;
  for(i = nrl + 1; i <= nrh; i++)
    m[i] = m[i - 1] + ncol;
  return m;
}
 
 
void
FilterSavitzkyGolay::free_ivector(int *v,
                                  long nl,
                                  [[maybe_unused]] long nh) const
{
  free((char *)(v + nl - 1));
}
 
 
void
FilterSavitzkyGolay::free_dvector(pappso_double *v,
                                  long nl,
                                  [[maybe_unused]] long nh) const
{
  free((char *)(v + nl - 1));
}
 
 
void
FilterSavitzkyGolay::free_dmatrix(pappso_double **m,
                                  long nrl,
                                  [[maybe_unused]] long nrh,
                                  long ncl,
                                  [[maybe_unused]] long nch) const
{
  free((char *)(m[nrl] + ncl - 1));
  free((char *)(m + nrl - 1));
}
 
 
void
FilterSavitzkyGolay::lubksb(pappso_double **a,
                            int n,
                            int *indx,
                            pappso_double b[]) const
{
  int i, ii = 0, ip, j;
  pappso_double sum;
 
  for(i = 1; i <= n; i++)
    {
      ip    = indx[i];
      sum   = b[ip];
      b[ip] = b[i];
      if(ii)
        for(j = ii; j <= i - 1; j++)
          sum -= a[i][j] * b[j];
      else if(sum)
        ii = i;
      b[i] = sum;
    }
  for(i = n; i >= 1; i--)
    {
      sum = b[i];
      for(j = i + 1; j <= n; j++)
        sum -= a[i][j] * b[j];
      b[i] = sum / a[i][i];
    }
}
 
 
void
FilterSavitzkyGolay::ludcmp(pappso_double **a,
                            int n,
                            int *indx,
                            pappso_double *d) const
{
  int i, imax = 0, j, k;
  pappso_double big, dum, sum, temp;
  pappso_double *vv;
 
  vv = dvector(1, n);
  *d = 1.0;
  for(i = 1; i <= n; i++)
    {
      big = 0.0;
      for(j = 1; j <= n; j++)
        if((temp = fabs(a[i][j])) > big)
          big = temp;
      if(big == 0.0)
        {
          qFatal("Error: Singular matrix found in routine ludcmp().");
        }
      vv[i] = 1.0 / big;
    }
  for(j = 1; j <= n; j++)
    {
      for(i = 1; i < j; i++)
        {
          sum = a[i][j];
          for(k = 1; k < i; k++)
            sum -= a[i][k] * a[k][j];
          a[i][j] = sum;
        }
      big = 0.0;
      for(i = j; i <= n; i++)
        {
          sum = a[i][j];
          for(k = 1; k < j; k++)
            sum -= a[i][k] * a[k][j];
          a[i][j] = sum;
          if((dum = vv[i] * fabs(sum)) >= big)
            {
              big  = dum;
              imax = i;
            }
        }
      if(j != imax)
        {
          for(k = 1; k <= n; k++)
            {
              dum        = a[imax][k];
              a[imax][k] = a[j][k];
              a[j][k]    = dum;
            }
          *d       = -(*d);
          vv[imax] = vv[j];
        }
      indx[j] = imax;
      if(a[j][j] == 0.0)
        a[j][j] = std::numeric_limits<pappso_double>::epsilon();
      if(j != n)
        {
          dum = 1.0 / (a[j][j]);
          for(i = j + 1; i <= n; i++)
            a[i][j] *= dum;
        }
    }
  free_dvector(vv, 1, n);
}
 
 
void
FilterSavitzkyGolay::four1(pappso_double data[], unsigned long nn, int isign)
{
  unsigned long n, mmax, m, j, istep, i;
  pappso_double wtemp, wr, wpr, wpi, wi, theta;
  pappso_double tempr, tempi;
 
  n = nn << 1;
  j = 1;
  for(i = 1; i < n; i += 2)
    {
      if(j > i)
        {
          SWAP(data[j], data[i]);
          SWAP(data[j + 1], data[i + 1]);
        }
      m = n >> 1;
      while(m >= 2 && j > m)
        {
          j -= m;
          m >>= 1;
        }
      j += m;
    }
  mmax = 2;
  while(n > mmax)
    {
      istep = mmax << 1;
      theta = isign * (6.28318530717959 / mmax);
      wtemp = sin(0.5 * theta);
      wpr   = -2.0 * wtemp * wtemp;
      wpi   = sin(theta);
      wr    = 1.0;
      wi    = 0.0;
      for(m = 1; m < mmax; m += 2)
        {
          for(i = m; i <= n; i += istep)
            {
              j           = i + mmax;
              tempr       = wr * data[j] - wi * data[j + 1];
              tempi       = wr * data[j + 1] + wi * data[j];
              data[j]     = data[i] - tempr;
              data[j + 1] = data[i + 1] - tempi;
              data[i] += tempr;
              data[i + 1] += tempi;
            }
          wr = (wtemp = wr) * wpr - wi * wpi + wr;
          wi = wi * wpr + wtemp * wpi + wi;
        }
      mmax = istep;
    }
}
 
 
void
FilterSavitzkyGolay::twofft(pappso_double data1[],
                            pappso_double data2[],
                            pappso_double fft1[],
                            pappso_double fft2[],
                            unsigned long n)
{
  unsigned long nn3, nn2, jj, j;
  pappso_double rep, rem, aip, aim;
 
  nn3 = 1 + (nn2 = 2 + n + n);
  for(j = 1, jj = 2; j <= n; j++, jj += 2)
    {
      fft1[jj - 1] = data1[j];
      fft1[jj]     = data2[j];
    }
  four1(fft1, n, 1);
  fft2[1] = fft1[2];
  fft1[2] = fft2[2] = 0.0;
  for(j = 3; j <= n + 1; j += 2)
    {
      rep           = 0.5 * (fft1[j] + fft1[nn2 - j]);
      rem           = 0.5 * (fft1[j] - fft1[nn2 - j]);
      aip           = 0.5 * (fft1[j + 1] + fft1[nn3 - j]);
      aim           = 0.5 * (fft1[j + 1] - fft1[nn3 - j]);
      fft1[j]       = rep;
      fft1[j + 1]   = aim;
      fft1[nn2 - j] = rep;
      fft1[nn3 - j] = -aim;
      fft2[j]       = aip;
      fft2[j + 1]   = -rem;
      fft2[nn2 - j] = aip;
      fft2[nn3 - j] = rem;
    }
}
 
 
void
FilterSavitzkyGolay::realft(pappso_double data[], unsigned long n, int isign)
{
  unsigned long i, i1, i2, i3, i4, np3;
  pappso_double c1 = 0.5, c2, h1r, h1i, h2r, h2i;
  pappso_double wr, wi, wpr, wpi, wtemp, theta;
 
  theta = 3.141592653589793 / (pappso_double)(n >> 1);
  if(isign == 1)
    {
      c2 = -0.5;
      four1(data, n >> 1, 1);
    }
  else
    {
      c2    = 0.5;
      theta = -theta;
    }
  wtemp = sin(0.5 * theta);
  wpr   = -2.0 * wtemp * wtemp;
  wpi   = sin(theta);
  wr    = 1.0 + wpr;
  wi    = wpi;
  np3   = n + 3;
  for(i = 2; i <= (n >> 2); i++)
    {
      i4       = 1 + (i3 = np3 - (i2 = 1 + (i1 = i + i - 1)));
      h1r      = c1 * (data[i1] + data[i3]);
      h1i      = c1 * (data[i2] - data[i4]);
      h2r      = -c2 * (data[i2] + data[i4]);
      h2i      = c2 * (data[i1] - data[i3]);
      data[i1] = h1r + wr * h2r - wi * h2i;
      data[i2] = h1i + wr * h2i + wi * h2r;
      data[i3] = h1r - wr * h2r + wi * h2i;
      data[i4] = -h1i + wr * h2i + wi * h2r;
      wr       = (wtemp = wr) * wpr - wi * wpi + wr;
      wi       = wi * wpr + wtemp * wpi + wi;
    }
  if(isign == 1)
    {
      data[1] = (h1r = data[1]) + data[2];
      data[2] = h1r - data[2];
    }
  else
    {
      data[1] = c1 * ((h1r = data[1]) + data[2]);
      data[2] = c1 * (h1r - data[2]);
      four1(data, n >> 1, -1);
    }
}
 
 
char
FilterSavitzkyGolay::convlv(pappso_double data[],
                            unsigned long n,
                            pappso_double respns[],
                            unsigned long m,
                            int isign,
                            pappso_double ans[])
{
  unsigned long i, no2;
  pappso_double dum, mag2, *fft;
 
  fft = dvector(1, n << 1);
  for(i = 1; i <= (m - 1) / 2; i++)
    respns[n + 1 - i] = respns[m + 1 - i];
  for(i = (m + 3) / 2; i <= n - (m - 1) / 2; i++)
    respns[i] = 0.0;
  twofft(data, respns, fft, ans, n);
  no2 = n >> 1;
  for(i = 2; i <= n + 2; i += 2)
    {
      if(isign == 1)
        {
          ans[i - 1] =
            (fft[i - 1] * (dum = ans[i - 1]) - fft[i] * ans[i]) / no2;
          ans[i] = (fft[i] * dum + fft[i - 1] * ans[i]) / no2;
        }
      else if(isign == -1)
        {
          if((mag2 = ans[i - 1] * ans[i - 1] + ans[i] * ans[i]) == 0.0)
            {
              qDebug("Attempt of deconvolving at zero response in convlv().");
              return (1);
            }
          ans[i - 1] =
            (fft[i - 1] * (dum = ans[i - 1]) + fft[i] * ans[i]) / mag2 / no2;
          ans[i] = (fft[i] * dum - fft[i - 1] * ans[i]) / mag2 / no2;
        }
      else
        {
          qDebug("No meaning for isign in convlv().");
          return (1);
        }
    }
  ans[2] = ans[n + 1];
  realft(ans, n, -1);
  free_dvector(fft, 1, n << 1);
  return (0);
}
 
 
char
FilterSavitzkyGolay::sgcoeff(
  pappso_double c[], int np, int nl, int nr, int ld, int m) const
{
  int imj, ipj, j, k, kk, mm, *indx;
  pappso_double d, fac, sum, **a, *b;
 
  if(np < nl + nr + 1 || nl < 0 || nr < 0 || ld > m || nl + nr < m)
    {
      qDebug("Inconsistent arguments detected in routine sgcoeff.");
      return (1);
    }
  indx = ivector(1, m + 1);
  a    = dmatrix(1, m + 1, 1, m + 1);
  b    = dvector(1, m + 1);
  for(ipj = 0; ipj <= (m << 1); ipj++)
    {
      sum = (ipj ? 0.0 : 1.0);
      for(k = 1; k <= nr; k++)
        sum += pow((pappso_double)k, (pappso_double)ipj);
      for(k = 1; k <= nl; k++)
        sum += pow((pappso_double)-k, (pappso_double)ipj);
      mm = (ipj < 2 * m - ipj ? ipj : 2 * m - ipj);
      for(imj = -mm; imj <= mm; imj += 2)
        a[1 + (ipj + imj) / 2][1 + (ipj - imj) / 2] = sum;
    }
  ludcmp(a, m + 1, indx, &d);
  for(j = 1; j <= m + 1; j++)
    b[j] = 0.0;
  b[ld + 1] = 1.0;
  lubksb(a, m + 1, indx, b);
  for(kk = 1; kk <= np; kk++)
    c[kk] = 0.0;
  for(k = -nl; k <= nr; k++)
    {
      sum = b[1];
      fac = 1.0;
      for(mm = 1; mm <= m; mm++)
        sum += b[mm + 1] * (fac *= k);
      kk    = ((np - k) % np) + 1;
      c[kk] = sum;
    }
  free_dvector(b, 1, m + 1);
  free_dmatrix(a, 1, m + 1, 1, m + 1);
  free_ivector(indx, 1, m + 1);
  return (0);
}
 
 
//! Perform the Savitzky-Golay filtering process.
/*
   The results are in the \c y_filtered_data_p C array of pappso_double
   values.
   */
char
FilterSavitzkyGolay::runFilter(double *y_data_p,
                               double *y_filtered_data_p,
                               int data_point_count) const
{
  int np = m_params.nL + 1 + m_params.nR;
  pappso_double *c;
  char retval;
 
#if CONVOLVE_WITH_NR_CONVLV
  c      = dvector(1, data_point_count);
  retval = sgcoeff(c, np, m_nL, m_nR, m_lD, m_m);
  if(retval == 0)
    convlv(y_data_p, data_point_count, c, np, 1, y_filtered_data_p);
  free_dvector(c, 1, data_point_count);
#else
  int j;
  long int k;
  c      = dvector(1, m_params.nL + m_params.nR + 1);
  retval = sgcoeff(c, np, m_params.nL, m_params.nR, m_params.lD, m_params.m);
  if(retval == 0)
    {
      qDebug() << __FILE__ << __LINE__ << __FUNCTION__ << "()"
               << "retval is 0";
 
      for(k = 1; k <= m_params.nL; k++)
        {
          for(y_filtered_data_p[k] = 0.0, j = -m_params.nL; j <= m_params.nR;
              j++)
            {
              if(k + j >= 1)
                {
                  y_filtered_data_p[k] +=
                    c[(j >= 0 ? j + 1 : m_params.nR + m_params.nL + 2 + j)] *
                    y_data_p[k + j];
                }
            }
        }
      for(k = m_params.nL + 1; k <= data_point_count - m_params.nR; k++)
        {
          for(y_filtered_data_p[k] = 0.0, j = -m_params.nL; j <= m_params.nR;
              j++)
            {
              y_filtered_data_p[k] +=
                c[(j >= 0 ? j + 1 : m_params.nR + m_params.nL + 2 + j)] *
                y_data_p[k + j];
            }
        }
      for(k = data_point_count - m_params.nR + 1; k <= data_point_count; k++)
        {
          for(y_filtered_data_p[k] = 0.0, j = -m_params.nL; j <= m_params.nR;
              j++)
            {
              if(k + j <= data_point_count)
                {
                  y_filtered_data_p[k] +=
                    c[(j >= 0 ? j + 1 : m_params.nR + m_params.nL + 2 + j)] *
                    y_data_p[k + j];
                }
            }
        }
    }
 
  free_dvector(c, 1, m_params.nR + m_params.nL + 1);
#endif
 
  return (retval);
}
 
 
} // namespace pappso