TRobustLineFitter.cxx

Go to the documentation of this file.

//-----------------------------------------------------------------------------
// $Id: TRobustLineFitter.cxx,v 1.7 2010/03/31 09:58:59 liucy Exp $
//-----------------------------------------------------------------------------
// Filename : TRobustLineFitter.cc
// Section  : Tracking
// Owner    : Yoshi Iwasaki
// Email    : yoshihito.iwasaki@kek.jp
//-----------------------------------------------------------------------------
// Description : A class to fit a TTrackBase object to a line.
//               See http://bsunsrv1.kek.jp/~yiwasaki/tracking/
//-----------------------------------------------------------------------------
 
/* for copysign */
#if defined( __sparc )
#  if defined( __EXTENSIONS__ )
#    include <cmath>
#  else
#    define __EXTENSIONS__
#    include <cmath>
#    undef __EXTENSIONS__
#  endif
#elif defined( __GNUC__ )
#  if defined( _XOPEN_SOURCE )
#    include <cmath>
#  else
#    define _XOPEN_SOURCE
#    include <cmath>
#    undef _XOPEN_SOURCE
#  endif
#endif
 
#include "TrkReco/TMLine.h"
#include "TrkReco/TMLink.h"
#include "TrkReco/TRobustLineFitter.h"
#include "TrkReco/TTrackBase.h"
 
TRobustLineFitter::TRobustLineFitter( const std::string& name )
    : TLineFitter( name ), _a( 0. ), _b( 0. ), _det( 0. ) {}
 
TRobustLineFitter::~TRobustLineFitter() {}
 
int TRobustLineFitter::fit( TTrackBase& t ) const {
  //...Initial guess...
  int err = TLineFitter::fit( t );
  if ( err ) return err;
 
  //...Check # of hits...
  const AList<TMLink>& links = t.links();
  _n                         = links.length();
  if ( _n < 3 ) return 0;
 
  //...Standard deviation...
  _a           = TLineFitter::a();
  _b           = TLineFitter::b();
  _det         = TLineFitter::det();
  double chisq = 0.;
  for ( unsigned i = 0; i < _n; i++ )
  {
    const HepPoint3D& p   = links[i]->position();
    double            tmp = p.y() - ( _a * p.x() + _b );
    chisq += tmp * tmp;
  }
  double siga = sqrt( chisq / _det );
 
  //...Decide iteration step...
  double a1 = _a;
  double f1 = rofunc( t, a1 );
  double a2 = _a + copysign( 3.0 * siga, f1 );
  double f2 = rofunc( t, a2 );
 
  // if initial value f2 >f1, change the search direction
  if ( f1 * f2 > 0. && fabs( f2 ) > fabs( f1 ) )
  {
    a2 = _a - copysign( 3.0 * siga, f1 );
    f2 = rofunc( t, a2 );
  }
 
  int backwardSearch = 0;
  while ( f1 * f2 > 0. )
  {
    _a = 2.0 * a2 - a1;
    a1 = a2;
    f1 = f2;
    a2 = _a;
    f2 = rofunc( t, a2 );
 
    if ( f1 * f2 > 0. && ( fabs( f2 ) > fabs( f1 ) || fabs( f2 - f1 ) < 0.01 ) )
    {
      backwardSearch++;
      if ( backwardSearch == 2 ) { break; }
 
      double f = f2;
      a2       = a1;
      f2       = f1;
      a1       = _a;
      f1       = f;
    }
  }
 
  if ( backwardSearch == 2 )
  {
    // for case of  no zero cross
    // search minimun fabs(f)
 
    double siga1 = 0.01 * siga;
    double a21( fabs( a2 - a1 ) );
    while ( a21 > siga1 )
    {
      _a = 0.5 * ( a1 + a2 );
      if ( _a == a1 || _a == a2 ) break;
      double f = rofunc( t, _a );
 
      if ( f * f1 < 0 )
      {
        f1 = f;
        a1 = _a;
        backwardSearch--;
        break;
      }
 
      if ( fabs( f ) <= fabs( f1 ) && fabs( f ) <= fabs( f2 ) )
      {
        if ( fabs( f - f1 ) > fabs( f - f2 ) )
        {
          f1 = f;
          a1 = _a;
        }
        else
        {
          f2 = f;
          a2 = _a;
        }
      }
      else if ( fabs( f ) <= fabs( f1 ) )
      {
        f1 = f;
        a1 = _a;
      }
      else if ( fabs( f ) <= fabs( f2 ) )
      {
        f2 = f;
        a2 = _a;
      }
      else
      {
        if ( fabs( f2 ) > fabs( f1 ) )
        {
          f1 = f;
          a1 = _a;
        }
        else
        {
          f2 = f;
          a2 = _a;
        }
      }
      if ( fabs( a2 - a1 ) >= a21 ) break;
      a21 = fabs( a2 - a1 );
    }
  }
 
  if ( backwardSearch <= 1 )
  {
 
    // search zero cross
    siga = 0.01 * siga;
 
    double a21( fabs( a2 - a1 ) );
    while ( a21 > siga )
    {
      _a = 0.5 * ( a1 + a2 );
      if ( _a == a1 || _a == a2 ) break;
      double f = rofunc( t, _a );
      if ( f * f1 >= 0. )
      {
        f1 = f;
        a1 = _a;
      }
      else
      {
        f2 = f;
        a2 = _a;
      }
      if ( fabs( a2 - a1 ) >= a21 ) break;
      a21 = fabs( a2 - a1 );
    }
  }
 
  _det = _det / double( _n );
 
  if ( t.objectType() == Line ) ( (TMLine&)t ).property( _a, _b, _det );
  fitDone( t );
  return 0;
}
 
double TRobustLineFitter::rofunc( const TTrackBase& t, double a ) const {
  double* arr = (double*)malloc( _n * sizeof( double ) );
  for ( unsigned i = 0; i < _n; i++ )
    arr[i] = t.links()[i]->position().y() - a * t.links()[i]->position().x();
  if ( _n & 1 ) { _b = select( ( _n + 1 ) >> 1, _n, arr ); }
  else
  {
    unsigned j = _n >> 1;
    _b         = 0.5 * ( select( j, _n, arr ) + select( j + 1, _n, arr ) );
  }
 
  _det       = 0.;
  double sum = 0.;
  for ( unsigned i = 0; i < _n; i++ )
  {
    double x = t.links()[i]->position().x();
    double y = t.links()[i]->position().y();
    double d = y - ( a * x + _b );
    _det += fabs( d );
    if ( y != 0. ) d /= fabs( y );
    if ( fabs( d ) > 1.0e-7 ) sum += d > 0.0 ? x : -x;
  }
  free( arr );
  return sum;
}
 
#define SWAP( a, b )                                                                          \
  tmp   = ( a );                                                                              \
  ( a ) = ( b );                                                                              \
  ( b ) = tmp;
 
double TRobustLineFitter::select( unsigned k, unsigned n, double* arr ) const {
  --k;
  double   tmp;
  unsigned l  = 0;
  unsigned ir = n - 1;
  while ( 1 )
  {
    if ( ir <= l + 1 )
    {
      if ( ir == l + 1 && arr[ir] < arr[l] ) { SWAP( arr[l], arr[ir] ); }
 
      //        std::cout << "k = " << k << std::endl;
      //        for (unsigned i = 0; i < _n; i++)
      //        std::cout << i << " : " << arr[i] << std::endl;
 
      return arr[k];
    }
    else
    {
      unsigned mid = ( l + ir ) >> 1;
      SWAP( arr[mid], arr[l + 1] );
      if ( arr[l + 1] > arr[ir] ) { SWAP( arr[l + 1], arr[ir] ); }
      if ( arr[l] > arr[ir] ) { SWAP( arr[l], arr[ir] ); }
      if ( arr[l + 1] > arr[l] ) { SWAP( arr[l + 1], arr[l] ); }
      unsigned i = l + 1;
      unsigned j = ir;
      double   a = arr[l];
      while ( 1 )
      {
        //          do i++; while (arr[i] < a);
        //          do j--; while (arr[j] > a);
        //          while (arr[i] < a) ++i;
        //          while (arr[j] > a) --j;
        while ( arr[++i] < a )
          ;
        while ( arr[--j] > a )
          ;
        if ( j < i ) break;
        SWAP( arr[i], arr[j] );
      }
      arr[l] = arr[j];
      arr[j] = a;
      if ( j >= k ) ir = j - 1;
      if ( j <= k ) l = i;
    }
  }
}