Ticket #837: base_lowess.cc

//
// BioArray Software Environment (BASE) - homepage http://base.thep.lu.se/
// Copyright (C) 2002,2003 Björn Samuelsson
//
// This file is part of BASE.
//
// BASE 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 2
// of the License, or (at your option) any later version.
//
// BASE 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, write to the Free Software
// Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.
//
#include <iostream>
#include <base_lowess.h>

namespace BASE
{

const double lowess::yw_range_factor = 6.;

static inline double cube(double a)
{
  return a * a * a;
}

double lowess::cubic_weight_func(double a)
{
  if(fabs(a) < 1.)
    return cube(1. - cube(fabs(a)));
  else
    return 0.;
}

static inline double sqr(double a)
{
  return a * a;
}

double lowess::quadratic_weight_func(double a)
{
  if(fabs(a) < 1.)
    return sqr(1. - sqr(a));
  else
    return 0.;
}


bool lowess::line_fit(
  double *x_beg, int npoints, double *y_beg, double &k, double &m,
  double *w_beg)
{
  const int np = npoints;
  if(np <= 0)
  {
    std::cerr << "line_fit called with npoints <= 0\n";
    exit(1);
  }
  
  double x_sum = 0., y_sum = 0., xx_sum = 0., xy_sum = 0., w_sum = 0.;

  if(w_beg)
  {
    for(int i = 0; i < np; i++)
    {
      double w = w_beg[i];
      x_sum += x_beg[i] * w;
      y_sum += y_beg[i] * w;
      xx_sum += x_beg[i] * x_beg[i] * w;
      xy_sum += x_beg[i] * y_beg[i] * w;
      w_sum += w;
    }
  }
  else
  {
    for(int i = 0; i < np; i++)
    {
      x_sum += x_beg[i];
      y_sum += y_beg[i];
      xx_sum += x_beg[i] * x_beg[i];
      xy_sum += x_beg[i] * y_beg[i];
    }
    w_sum = np;
  }
  
  if(w_sum <= 0.)
    return false;

  double denom = w_sum*xx_sum - x_sum*x_sum;
  if(denom != 0.)
  {
    k = (w_sum*xy_sum - x_sum*y_sum) / denom;
    m = (y_sum - k*x_sum) / w_sum;
  }
  else
  {
    k = 0.;
    m = y_sum / w_sum;
  }
  return true;
}

// fit for one point
// x must be sorted in ascending order
bool lowess::p_lowess(
  double *x_beg, int npoints, double *y_beg,
  const double &p_x, double &p_y, double *temp_w_beg,
  double *yw_beg, weight_func x_weight_func)
{
  const int np = npoints;
  if(np <= 0)
  {
    std::cerr << "p_lowess called with npoints <= 0\n";
    exit(1);
  }

  double inv_radius = 1. / std::max(p_x - *x_beg, x_beg[np - 1] - p_x);
  if(yw_beg)
  {
    for(int i = 0; i < np; i++)
    {
      temp_w_beg[i] = (*x_weight_func)((x_beg[i] - p_x) * inv_radius) *
        yw_beg[i];
    }
  }
  else
  {
    for(int i = 0; i < np; i++)
      temp_w_beg[i] = x_weight_func((x_beg[i] - p_x) * inv_radius);
  }

  double k, m;
  bool tmp = line_fit(x_beg, npoints, y_beg, k, m, temp_w_beg);

  p_y = k * p_x + m;
  return tmp;
}

// robust fit of line segments
// x must be sorted in ascending order
void lowess::calc(
  double *x_beg, int npoints, double *y_beg,
  double *y_fit_beg, char *point_fitted_beg,
  const double &frac, const int n_iter, const double &delta,
  weight_func x_weight_func, weight_func y_weight_func)
{
  const int np_window = std::min(npoints, int(frac*npoints + 0.5));
  if(npoints == 0)
    return;
  if(npoints < 0)
  {
    std::cerr << "lowess called with npoints < 0\n";
    exit(1);
  }

  if(np_window <= 2)
  {
    std::copy(y_beg, y_beg + npoints, y_fit_beg);
    std::fill(point_fitted_beg, point_fitted_beg + npoints, true);
    return;
  }

  double w_beg[npoints];
  double temp_beg[npoints];

  for(int iter = 0; iter <= n_iter; iter++)
  {
    int window_i = 0;
    int i, prev_i;
    int misses = 0, pcalcs = 0;
  
    for(i = 0, prev_i = -1; prev_i < npoints - 1;)
    {
      //center window
      while(window_i + np_window < npoints && 
        x_beg[i] - x_beg[window_i] > 
        x_beg[window_i + np_window - 1] - x_beg[i])
      {
        window_i++;
      }

      //fit one point
      pcalcs++;
      bool fitted = p_lowess(x_beg + window_i, np_window,
        y_beg + window_i, x_beg[i], y_fit_beg[i], temp_beg,
        iter ? w_beg + window_i : 0, x_weight_func);

      if(!fitted) 
      {
        misses++;
        if(i < npoints - 1)
          ;
        else if(prev_i < 0)
        {
          std::cerr << "Not a single point fitted - this is an odd error.\n"
            "Please report it as a bug.\n";
          exit(1);
        }
        else
        {
          for(int j = prev_i + 1; j < i; j++)
            y_fit_beg[j] = y_fit_beg[prev_i];
          break;
        }
      }
      else
      {
        //interpolate skipped points
        if(prev_i + 1 < i)
        {
          double d = prev_i < 0 ? 0 : x_beg[i] - x_beg[prev_i];
          if(d == 0.)
          {
            for(int j = prev_i + 1; j < i; j++)
              y_fit_beg[j] = y_fit_beg[i];
          }
          else
          {
            for(int j = prev_i + 1; j < i; j++)
            {
              double a = (x_beg[j] - x_beg[prev_i])/d;
              y_fit_beg[j] = a*y_fit_beg[i] + (1.-a)*y_fit_beg[prev_i];
            }
          }
        }

        for(int j = prev_i + 1; j < i; j++)
          point_fitted_beg[j] = false;
        point_fitted_beg[i] = true;

        prev_i = i;
      }

      double cut = x_beg[i] + delta;
      for(i++; i < npoints && x_beg[i] <= cut; i++)
      {
        if(x_beg[i] - x_beg[i-1] < 0)
        {
          std::cerr << "x-values not sorted in lowess\n";
          exit(1);
        }
      }
      if(i >= npoints)
        i = npoints - 1;
    }

    if(misses)
    {
      std::cerr << "Warning: sum of weights was 0 in line_fit " << misses
        << " times (of " << pcalcs << ") in iteration " << iter << ".\n"
        "This happens if all points within the sliding window are\n"
        "more than 6 medians from the locally fitted function.\n";
    }
    
    for(int i = 0; i < npoints; i++)
      temp_beg[i] = fabs(y_fit_beg[i] - y_beg[i]);

    std::pair<double*, double*> p = median(temp_beg, temp_beg + npoints);
    double inv_yw_range = 1. /
      (yw_range_factor * 0.5 * (*p.first + *p.second));
    for(int i = 0; i < npoints; i++)
      w_beg[i] = y_weight_func((y_fit_beg[i] - y_beg[i]) * inv_yw_range);
  }
}

}