LocalFit.cxx

Go to the documentation of this file.

// $Id: LocalFit.cxx,v 1.2 2009/02/28 21:46:15 gmieg Exp $

// C++
#include <algorithm>
#include <cassert>
#include <cmath>
#include <iostream>

// MINOS
#include "Registry/Registry.h"

// Local
#include "LocalFit.h"

using namespace std;

//-----------------------------------------------------------------------------------------
Anp::LocalFit::LocalFit()
   :fDebug(false),
    fValid(false),
    fDegree(1),
    fIter(3),
    fWindow(7)
{
}

//-----------------------------------------------------------------------------------------
Anp::LocalFit::~LocalFit()
{
}

//-----------------------------------------------------------------------------------------
bool Anp::LocalFit::Add(const int index, const double x_, const double y_)
{
   if(std::find(fFitVec.begin(), fFitVec.end(), index) != fFitVec.end())
   {
      cerr << "LocalFit::Add - index already exists: " << index << endl;
      return false;
   }

   fFitVec.push_back(Anp::FitPoint(index, x_, y_));
   return false;
}

//-----------------------------------------------------------------------------------------
bool Anp::LocalFit::RunFit()
{
   //
   // Fit locally data points with 2nd or 3rd degree polynomial
   // 

   if(fDebug)
   {
      cout << "LocalFit::RunFit - starting new fit" << endl;
   }

   //
   // Check validity of data in fit vector
   //
   if(!SortCleanCheck(fFitVec))
   {
      cerr << "LocalFit::RunFit - invalid fit vector" << endl;
      return false;
   }
   
   if(fDebug)
   {
      cout << "LocalFit::RunFit - fit vector size = " <<  fFitVec.size() << endl;
   }

   // 
   // For each point get list of fWindow nearest points and set local weights
   //
   for(unsigned int ipos = 0; ipos < fFitVec.size(); ++ipos)
   {
      const List nlist = GetList(ipos);

      if(nlist.empty() || 
         nlist.size() > fWindow || 
         (fFitVec.size() > fWindow && nlist.size() != fWindow))
      {
         cerr << "LocalFit::RunFit - failed to find " << fWindow << " neighbors" << endl;
         return false;
      }

      FitPoint &point = fFitVec[ipos];

      const double width = nlist.back().second;
      if(!(width > 0.0))
      {
         cerr << "LocalFit::RunFit - interval size is zero" << endl;
         return false;   
      }
      
      for(List::const_iterator lit = nlist.begin(); lit != nlist.end(); ++lit)
      {
         point.SetWeight(lit -> first, WFunction(lit -> second/width));
      }
   }

   if(fDebug)
   {
      cout << "LocalFit::RunFit - starting " << fIter << " iterations..." << endl;
   }

   for(unsigned int i = 0; i <= fIter; ++i)
   {  
      std::vector<double> resvec;
      for(vector<FitPoint>::iterator fit = fFitVec.begin(); fit != fFitVec.end(); ++fit)
      {
         if(fDegree == 1)
         {
            RunLFit(*fit, fFitVec);
         }
         else if(fDegree == 2)
         {
            RunQFit(*fit, fFitVec);
         }
         else
         {
            cerr << "LocalFit::RunFit - uknown polynomial degree: " << fDegree << endl;
            return false;
         }

         if(fit -> Pass())
         {
            resvec.push_back(std::fabs(fit -> Residual()));
         }
      }
      
      if(resvec.size() != fFitVec.size())
      {
         cerr << "LocalFit::RunFit - # of failed points = " << fFitVec.size() -  resvec.size() << endl;
         for(vector<FitPoint>::iterator fit = fFitVec.begin(); fit != fFitVec.end(); ++fit)
         {
            if(!(fit -> Pass()))
            {
               cout << "   ";
               fit -> Print();
            }
         }
         return false;
      }
/*
      std::sort(resvec.begin(), resvec.end());
      const double median = resvec[resvec.size()/2];

      if(!(median > 0.0))
      {
         cerr << "LocalFit::RunFit - median residual is zero" << endl;
         return false;
      }      
      
      for(vector<FitPoint>::iterator fit = fFitVec.begin(); fit != fFitVec.end(); ++fit)
      {
         fit -> SetWeight(BFunction(fit -> Residual()/(6.0*median)));
      }
*/

      const double maxres = *std::max_element(resvec.begin(), resvec.end());

      if(!(maxres > 0.0))
      {
         cerr << "LocalFit::RunFit - maximum residual is zero" << endl;
         return false;
      }      
      
      for(vector<FitPoint>::iterator fit = fFitVec.begin(); fit != fFitVec.end(); ++fit)
      {
         fit -> SetWeight(BFunction(fit -> Residual()/maxres));
      }
   }

   if(fDebug)
   {
      cout << "LocalFit::RunFit - finished" << endl;
   }

   fValid = true;

   return true;
}

//-----------------------------------------------------------------------------------------
void Anp::LocalFit::Clear()
{
   fFitVec.clear();
   fValid = false;
}

//----------------------------------------------------------------------
void Anp::LocalFit::Config(const Registry &reg)
{
   const char *value_char = 0;
   if(reg.Get("LocalFitDebug", value_char) && value_char)
   {
      if(strcmp(value_char, "yes") == 0)
      {
         fDebug = true;
      }
      else if(strcmp(value_char, "no") == 0)
      {
         fDebug = false;
      }
   }

   int value_int = 0;
   if(reg.Get("LocalFitDegree", value_int))
   {
      SetDegree(value_int);
   }

   value_int = 0;
   if(reg.Get("LocalFitWindow", value_int))
   {
      SetWindow(value_int);
   }

   value_int = 0;
   if(reg.Get("LocalFitIter", value_int))
   {
      SetIter(value_int);
   }

   value_char = 0;
   if(reg.Get("PrintConfig", value_char) && value_char && strcmp(value_char, "yes") == 0)
   {
      cout << "LocalFit::Config" << endl
           << "   Debug = " << fDebug << endl
           << "   Degree = " << fDegree << endl
           << "   Window = " << fWindow << endl
           << "   Iter = " << fIter << endl;
   }
}

//-----------------------------------------------------------------------------------------
double Anp::LocalFit::FitY(const double x) const
{
   if(!fValid || fFitVec.empty())
   {
      cerr << "LocalFit::FitY - I do not have a valid fit vector" << endl;
      return 0.0;
   }

   vector<FitPoint>::const_iterator fit = std::upper_bound(fFitVec.begin(), fFitVec.end(), x);
   if(fit == fFitVec.end())
   {
      cerr << "LocalFit::FitY - x = " << x << " is greater than upper bound of fit range " 
           << fFitVec.back().X() << endl;
   }
   else if(fit == fFitVec.begin())
   {
      cerr << "LocalFit::FitY - x = " << x << " is less than lower bound of fit range " 
           << fFitVec.front().X() << endl;
   }
   
   if(fit -> Pass())
   {
      return fit -> FitY(x);
   }

   cerr << "LocalFit::FitY - closest fit point failed local fit:" << endl << "   ";   
   fit -> Print(std::cerr);

   return fit -> Y();
}

//-----------------------------------------------------------------------------------------
void Anp::LocalFit::SetWindow(const int value)
{
   if(value < 2)
   {
      cerr << "LocalFit::SetWindow - invalid window parameter: " << value << endl;
   }
   
   fWindow = static_cast<unsigned int>(value);
}

//-----------------------------------------------------------------------------------------
void Anp::LocalFit::SetIter(int value)
{
   if(value < 0)
   {
      value = 0;
   }
   
   fIter = static_cast<unsigned int>(value);
}

//-----------------------------------------------------------------------------------------
void Anp::LocalFit::SetDegree(int value)
{
   if(value != 1 && value != 2)
   {
      cerr << "LocalFit::SetDegree - invalid degree parameter: " << value << endl;
   }
   
   fDegree = static_cast<unsigned int>(value);
}

//-----------------------------------------------------------------------------------------
void Anp::LocalFit::Print(ostream &os) const
{
   os << "LocalFit" << endl
      << "   Debug = " << fDebug << endl
      << "   Degree = " << fDegree << endl
      << "   Iter = " << fIter << endl
      << "   Window = " << fWindow << endl
      << "   Valid = " << fValid << endl;

   for(vector<FitPoint>::const_iterator fit = fFitVec.begin(); fit != fFitVec.end(); ++fit)
   {
      os << "   ";
      fit -> Print(os);
   }
}

//-----------------------------------------------------------------------------------------
bool Anp::LocalFit::RunLFit(FitPoint &point, const vector<FitPoint> &fvec) const
{

   point.SetL(0.0, point.Y(), false);

   const map<unsigned int, double> &wmap = point.GetWeightMap();

   double A = 0.0, B = 0.0, C = 0.0, D = 0.0, E = 0.0, F = 0.0;
   unsigned int wsize = 0;

   for(map<unsigned int, double>::const_iterator wit = wmap.begin(); wit != wmap.end(); ++wit)
   {
      const unsigned int index = wit -> first;

      if(index >= fvec.size())
      {
         cerr << "LocalFit::RunLFit - index of FitPoint is out of range" << endl;
         continue;
      }
      
      const FitPoint &fitpoint = fvec[index];

      const double x = fitpoint.X();
      const double y = fitpoint.Y();
      const double w = wit -> second * fitpoint.Weight();
      
      assert(!(fitpoint.Weight() < 0.0) && "Global weight is negative");

      // ignore points with zero weights
      if(!(w > 0.0))
      {
         continue;
      }

      ++wsize;
      
      A += w * x;
      B += w;
      C += w * y;
      D += w * x * x;
      E += w * x * y;
      F += w * y * y;
   }

   if(wsize < 2)
   {
      cerr << "LocalFit::RunLFit - too few fit points with positive weight" << endl;
      
      for(map<unsigned int, double>::const_iterator wit = wmap.begin(); wit != wmap.end(); ++wit)
      {
         cout << "   (index, weight) = (" << wit -> first << ", " << wit -> second << ")" << endl;
      }
      return false;
   }

   const double det = B * D - A * A;
   if(!(det > 0.0))
   {
      cerr << "LocalFit::RunLFit - linear fit failed for " << wsize << " points" << endl;
      return false;
   }
   
   const double a = (E * B - C * A)/det;
   const double b = (D * C - E * A)/det;

   point.SetL(a, b, true);

   return true;
}
//-----------------------------------------------------------------------------------------
bool Anp::LocalFit::RunQFit(FitPoint &point, const vector<FitPoint> &fvec) const
{

   point.SetQ(0.0, 0.0, point.Y(), false);

   const map<unsigned int, double> &wmap = point.GetWeightMap();

   double S0 = 0.0, S1 = 0.0, S2 = 0.0, S3 = 0.0, S4 = 0.0, M1 = 0.0, M2 = 0.0, M3 = 0.0;
   unsigned int wsize = 0;

   for(map<unsigned int, double>::const_iterator wit = wmap.begin(); wit != wmap.end(); ++wit)
   {
      const unsigned int index = wit -> first;
      if(!(index < fvec.size()))
      {
         cerr << "LocalFit::RunQFit - index of FitPoint is out of range" << endl;
         continue;
      }
      
      const FitPoint &fitpoint = fvec[index];

      const double x = fitpoint.X();
      const double y = fitpoint.Y();
      const double w = wit -> second * fitpoint.Weight();

      assert(!(fitpoint.Weight() < 0.0) && "Global weight is negative");

      // ignore points with zero weights
      if(!(w > 0.0))
      {
         continue;
      }

      ++wsize;

      double product_x = x;
      
      S0 += w;

      S1 += w * product_x;

      product_x *= x;
      S2 += w * product_x;

      product_x *= x;
      S3 += w * product_x;

      product_x *= x;
      S4 += w * product_x;

      double product_xy = y;

      M1 += w * product_xy;
    
      product_xy *= x;
      M2 += w * product_xy;  
      
      product_xy *= x;
      M3 += w * product_xy; 
   }

   if(wsize < 3)
   {
      cerr << "LocalFit::RunQFit - too few fit points with positive weight" << endl;
      for(map<unsigned int, double>::const_iterator wit = wmap.begin(); wit != wmap.end(); ++wit)
      {
         cout << "   (index, weight) = (" << wit -> first << ", " << wit -> second << ")" << endl;
      }
      return false;
   }
   
   const double det = S0*(S2*S4 - S3*S3) - S1*(S1*S4 - S2*S3) + S2*(S1*S3 - S2*S2);
   if(!(det > 0.0))
   {
      cerr << "LocalFit::RunQFit - quadratic fit failed for " << wsize << " points" << endl;
      return false;
   }

   const double a = (+M1*(S1*S3 - S2*S2) - M2*(S0*S3 - S1*S2) + M3*(S0*S2 - S1*S1))/det;   
   const double b = (-M1*(S1*S4 - S2*S3) + M2*(S0*S4 - S2*S2) - M3*(S0*S3 - S1*S2))/det;
   const double c = (+M1*(S2*S4 - S3*S3) - M2*(S1*S4 - S2*S3) + M3*(S1*S3 - S2*S2))/det;

   point.SetQ(a, b, c, true);

   return true;
}

//-----------------------------------------------------------------------------------------
bool Anp::LocalFit::SortCleanCheck(vector<FitPoint> &fvec) const
{
   if(fDebug)
   {
      cout << "LocalFit::SortCleanCheck - begin..." << endl;
   }

   //
   // Sort fit vector by x values
   //
   std::sort(fvec.begin(), fvec.end());

   //
   // Find and remove duplicate x values
   //
   vector<FitPoint>::iterator fit = fvec.begin();
   while(fit != fvec.end())
   {
      vector<FitPoint>::iterator nit = fit + 1;
      if(nit == fvec.end())
      {
         break;
      }

      if(!(fit -> X() > nit -> X()) && !(fit -> X() < nit -> X()))
      {
         cerr << "LocalFit::SortCleanCheck - removing duplicate x: " << nit -> X() << endl;
         fvec.erase(nit);
         fit = fvec.begin();
      }
      else
      {
         ++fit;
      }
   }

   //
   // Check that fit vector has enough point for regression of degree fDegree
   //
   if(fvec.size() < fDegree + 2)
   {
      cerr << "LocalFit::SortCleanCheck - too few points for regression: " << fvec.size() << endl;
      return false;
   }  

   if(fDebug)
   {
      cout << "LocalFit::SortCleanCheck - passed" << endl;
   }

   return true;
}

//-----------------------------------------------------------------------------------------
const Anp::LocalFit::List Anp::LocalFit::GetList(const unsigned int pos) const
{
   //
   // Return pair of position and distance for fWindow nearest points for point at pos.
   // Assume that fFitVec is sorted by X values, but no assumption is made
   // about relative distances between X values.
   //

   List nlist;

   if(pos >= fFitVec.size())
   {
      cerr << "LocalFit::GetList - position is out of range: " << pos << endl;
      return nlist;
   }

   if(fWindow < 1)
   {
      cerr << "LocalFit::GetList - window is too small " << fWindow << endl;
      return nlist;
   }

   const int ibeg = std::max<int>(int(pos) - int(fWindow + 0), 0);
   const int iend = std::min<int>(int(pos) + int(fWindow + 1), int(fFitVec.size()));   

   assert(ibeg >= 0 && iend <= int(fFitVec.size()) && "invalid range");

   if(fDebug)
   {
      cout << "LocalFit::GetList - range for searching (ibeg, iend) = (" 
           << ibeg << ", " << iend << ")" << endl;
   }

   const FitPoint &point = fFitVec[pos];

   for(int i = ibeg; i < iend; ++i)
   {
      // ignore this (anchor) point
      if(i == int(pos))
      {
         continue;
      }

      const FitPoint &other = fFitVec[i];
      const double dist = std::fabs(point.X() - other.X());
      
      if(!(dist > 0.0))
      {
         cerr << "LocalFit::GetList - ignore zero distance point" << endl;
         continue;
      }

      // get current farthest distance from anchor point
      // and ignore points outside current minimum range
      if(!nlist.empty() && nlist.size() == fWindow && !(dist < nlist.back().second))
      {
         continue;
      }

      List::iterator lit = nlist.begin();
      for(; lit != nlist.end(); ++lit)
      {
         if(dist < lit -> second)
         {
            break;
         }
         else
         {
            continue;
         }
      }
      
      nlist.insert(lit, pair<unsigned int,double>(i, dist));
      
      if(nlist.size() > fWindow)
      {
         nlist.pop_back();
      }
   }

   if(fDebug)
   {
      cout << "LocalFit::GetList - list of " << fWindow << " neighbors" << endl;
      cout << "   Anchor ";
      point.Print();
      
      for(List::const_iterator lit = nlist.begin(); lit != nlist.end(); ++lit)
      {
         cout << "   position = " << lit -> first << ", distance = " << lit -> second << endl;
      }
   }

   return nlist;
}

//-----------------------------------------------------------------------------------------
double Anp::LocalFit::BFunction(const double value) const
{
   if(!(std::fabs(value) < 1.0))
   {
      return 0.0;
   }
   
   const double prod = 1.0 - value * value;
   
   return (prod * prod);
}

//-----------------------------------------------------------------------------------------
double Anp::LocalFit::WFunction(const double value) const
{
   const double avalue = std::fabs(value);

   if(!(avalue < 1.0))
   {
      return 0.0;
   }  

   const double prod = 1.0 - avalue * avalue * avalue;

   return (prod * prod * prod);
}

---