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    : [email protected]
//-----------------------------------------------------------------------------
// 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/TRobustLineFitter.h"
#include "TrkReco/TTrackBase.h"
#include "TrkReco/TMLine.h"
#include "TrkReco/TMLink.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;
    }
    }
}