TrkHelixFitter.cxx

Go to the documentation of this file.

//--------------------------------------------------------------------------
// File and Version Information:
//      $Id: TrkHelixFitter.cxx,v 1.16 2011/09/26 00:56:56 zhangy Exp $
//
// Description:
//     Performs least-squares fit for any simple track representation --
// circle, line, helix.  Depending on the state of flags, it will also
// drop hits and flip ambiguities (for hits that allow such action) while
// fitting.  (Originally written in C, which shows in a few spots.)
//
// Environment:
//      Software developed for the BaBar Detector at the SLAC B-Factory.
//
// Author(s): Steve Schaffner
//
//------------------------------------------------------------------------
#include "TrkFitter/TrkHelixFitter.h"
#include "CLHEP/Geometry/Point3D.h"
#include "CLHEP/Matrix/Matrix.h"
#include "CLHEP/Matrix/SymMatrix.h"
#include "CLHEP/Matrix/Vector.h"
#include "TrkBase/TrkEnums.h"
#include "TrkBase/TrkErrCode.h"
#include "TrkBase/TrkHitOnTrk.h"
#include "TrkBase/TrkHotList.h"
#include "TrkBase/TrkParams.h"
#include "TrkBase/TrkSimpTraj.h"
#include <assert.h>
#include <vector>
using std::cout;
using std::endl;
 
bool   TrkHelixFitter::m_debug       = false;
double TrkHelixFitter::nSigmaCut[43] = {
    10., 5., 5., 10., 10., 5., 5., 10.,                                   // U,V
    10., 5., 5., 5.,  5.,  5., 5., 5.,  5., 5., 5., 10.,                  // A,A,A
    10., 5., 5., 5.,  5.,  5., 5., 5.,  5., 5., 5., 5.,  5., 5., 5., 10., // U,V,U,V
    10., 5., 5., 5.,  5.,  5., 10.                                        // A,A
};
//------------------------------------------------------------------------
TrkHelixFitter::~TrkHelixFitter() {}
//------------------------------------------------------------------------
 
//------------------------------------------------------------------------
TrkHelixFitter::TrkHelixFitter( bool allowFlips, bool allowDrops )
    : //------------------------------------------------------------------------
    TrkHitOnTrkUpdater() {
  _allowFlips = allowFlips;
  _allowDrops = allowDrops;
  _lastChisq  = -1.;
}
 
//------------------------------------------------------------------------
TrkHelixFitter::TrkHelixFitter( const TrkHelixFitter& right )
    : //------------------------------------------------------------------------
    TrkHitOnTrkUpdater() {
  _allowFlips = right._allowFlips;
  _allowDrops = right._allowDrops;
  _lastChisq  = -1.;
}
 
//------------------------------------------------------------------------
TrkHelixFitter& TrkHelixFitter::operator=( const TrkHelixFitter& right )
//------------------------------------------------------------------------
{
  if ( &right == this ) return *this;
  _allowFlips = right._allowFlips;
  _allowDrops = right._allowDrops;
  _lastChisq  = right._lastChisq;
 
  return *this;
}
 
//------------------------------------------------------------------------
void TrkHelixFitter::setFittingPar( bool allowFlips, bool allowDrops ) {
  //------------------------------------------------------------------------
  _allowFlips = allowFlips;
  _allowDrops = allowDrops;
}
 
//------------------------------------------------------------------------
TrkErrCode TrkHelixFitter::fit( TrkHotList& hitlist, TrkSimpTraj& theTraj ) {
  //------------------------------------------------------------------------
  // Assumes that weight matrix is diagonal. */
  /* Least-squares fit; the measured
     quantity is the residual.  The fit is accomplished by linearizing
     the equation, using the derivatives of the residual w/r/t the
     track parameters; because of this approximation, the fit may be iterated.
     The fitted parameters are given by:
     delta-param() = Vparam * Atran * Vyinv * delChi
     where Vyinv = covariance matrix for the measurements
     Atran = transpose of A
     A = matrix of derivatives of delChi wrt trk params
     (size = no. of params x no. of hits)
     Vparam = covariance (error)" matrix of the fitted parameters
     = (Atran * Vyinv * A)**-1
     */
 
  bool permitFlips = _allowFlips;
  bool lPickHits   = _allowDrops;
  // permitFlips = 1 => permit state changes like ambiguity flips
  // lPickHits = 1 => choose the best set of active hits on each iteration
  int        i;
  TrkErrCode status( TrkErrCode::succeed );
  int        lPicked = 0; // = 1 => have either picked up or dropped an active hit
  // on this iteration
  register double chisqold;
  double          chisqnew, chichange;
  double          chitest = 0.01;  // delta(chi2) < chitest => fit has converged
  int             nZ = 0, nXY = 0; //  # active hits in each view
  int             nActive = 0;
 
  // vparam = Vparam defined above ( = symmetric matrix)
  // diffsum = Atran * Vyinv * delChi defined above (column vector)
  // iter = iteration loop index
  // itermax = max number of iterations
  // delpar = change in parameters during this iteration
  // chisqold, chisqnew = chisq before and after latest iteration
 
  /***************************************************************************/
  setLastChisq( -1. );
  // bool shiftRef = false;//yzhang FIXME
  //   HepPoint3D storePoint;
 
  // Change reference point of trajectory to be at first hit -- reduces
  //   numerical problems
  //  double oldT0 = hitlist[0]->parentTrack()->trackT0();
  // if (shiftRef) {
  //    double firstFlight = hitlist[0]->fltLen();
  //    double newTime = hitlist[0]->parentTrack()->fitResult()->arrivalTime(firstFlight);
  //    hitlist[0]->parentTrack()->resetT0(newTime);
  //    Point3D.here = theTraj.position(firstFlight);
  //
  //    storePoint = here;
  //    DifPoint dfPos;
  //    DifVector dfDir, dfDelDir;
  //    theTraj.getDFInfo(firstFlight, dfPos, dfDir, dfDelDir);
  //
  //    theTraj.changePoint(here, fltOffset);
  //}
 
  //*** Things that don't change with each iteration
  int                              nhits = hitlist.nHit();
  std::vector<double>              delChi( nhits, 0 );
  std::vector<std::vector<double>> deriv( nhits );
 
  TrkParams& params = *( theTraj.parameters() );
  //  int npar = params.nPar();//yzhang temp
  int npar = theTraj.nPar(); // yzhang temp
 
  // Decide minimum numbers of hits required.  This could turn out to be wrong
  //     someday.
 
  bool      l3d    = ( npar > 3 ); // I hope always true
  const int minZ   = l3d ? 2 : 0;
  const int minXY  = npar - minZ;
  const int minAct = minZ + minXY;
 
  HepSymMatrix vparam( npar, 0 );
  HepVector    diffsum( npar );
  HepVector    delpar( npar );
 
  std::vector<std::vector<double>>::iterator ideriv  = deriv.begin();
  std::vector<double>::iterator              idelChi = delChi.begin();
  assert( ( (int)deriv.size() ) == ( hitlist.end() - hitlist.begin() ) );
  for ( TrkHotList::nc_hot_iterator ihit = hitlist.begin(); ihit != hitlist.end();
        ++ihit, ++ideriv, ++idelChi )
  {
    ideriv->resize( npar );
    if ( ihit->isActive() )
    {
      nActive++;
      if ( ihit->whatView() == TrkEnums::xyView ) nXY++;
      else if ( ihit->whatView() == TrkEnums::zView ) nZ++;
      else if ( ihit->whatView() == TrkEnums::bothView )
      {
        nZ++;
        nXY++;
      }
    }
 
    // Update the Hots to reflect new reference point
    // if (shiftRef) {
    //      ihit->setFltLen( ihit->fltLen() - fltOffset );
    //}
  } // end loop over hits
  if ( nXY < minXY || nZ < minZ || nActive < minAct )
  {
    status.setFailure( 11, "Not enough hits in TrkHelixFitter! " );
    return status;
  }
 
  // if (shiftRef) {
  //     double firstFlight = hitlist[0]->fltLen();
  //     Point3D.here = theTraj.position(firstFlight);
  //     DifPoint dfPos;
  //     DifVector dfDir, dfDelDir;
  //     theTraj.getDFInfo(firstFlight, dfPos, dfDir, dfDelDir);
  //     double dummy = 0.;
  //     hitlist[0]->updateFitStuff(dummy, 0, !permitFlips);
  //     hitlist[0]->updateMeasurement();
  //     hitlist[0]->updateFitStuff(dummy, 0, !permitFlips);
  // }
  //   static HepVector derivs(npar);//yzhang temp
  HepVector derivs( npar ); // zhang change
  //  HepVector derivs(npar);//yzhang temp
  TrkErrCode calcResult;
  //**** Iterate fit.
  size_t itermax = 12;
  for ( size_t iter = 1; iter <= itermax; iter++ )
  {
    bool mustIterate( false ); // flag to force another iteration
    chisqold = 0.0;
    for ( i = 0; i < npar; i++ ) diffsum[i] = 0.0;
    vparam *= 0.0; // dumb way of clearing matrix
 
    /* Loop over hits, accumulate sums, calculate chisq for current params. */
    std::vector<std::vector<double>>::iterator ideriv  = deriv.begin();
    std::vector<double>::iterator              idelChi = delChi.begin();
    assert( ( (int)deriv.size() ) == ( hitlist.end() - hitlist.begin() ) );
    for ( TrkHotList::nc_hot_iterator ihit = hitlist.begin(); ihit != hitlist.end();
          ++ihit, ++ideriv, ++idelChi )
    {
 
      // Ask the hit to do the calculations
      calcResult = updateMeasurement( *ihit, 0, !permitFlips );
      double deltaChiNew;
      if ( calcResult.success() )
      {
        if ( iter < 2 )
        { // FIXME? only update derivatives at first iteration...
          calcResult = ihit->getFitStuff( derivs, deltaChiNew );
          for ( i = 0; i < npar; ++i ) ( *ideriv )[i] = derivs[i];
          // if(m_debug){
          //   std::cout<<"in TrkHelixFitter "  <<std::endl;//yzhang deubg
          //   cout << "deriv: ";
          //   for (i=0;i<npar;++i) cout << (*ideriv)[i] << " " ;
          //   cout << endl;
          // }
        }
        else { calcResult = ihit->getFitStuff( deltaChiNew ); }
      }
      if ( calcResult.failure() )
      {
        if ( m_debug )
        {
          cout << "ErrMsg(warning) TrkHelixFitter:"
               << "unable to getFitStuff for hit " << *ihit << endl;
        }
        ihit->setUsability( false ); // something bombed
        continue;
      }
      mustIterate = ( mustIterate || ( calcResult.success() != 1 ) );
      *idelChi    = deltaChiNew;
      if ( m_debug )
      {
        cout << ( ihit - hitlist.begin() );
        ihit->print( std::cout );
        cout << " dChi " << *idelChi << " amb " << ihit->ambig() << " resid " << ihit->resid()
             << " rms " << ihit->hitRms() << " hitlen " << ihit->hitLen() << " fltlen "
             << ihit->fltLen() << endl;
      }
      if ( ihit->isActive() == false )
      {
        if ( m_debug ) std::cout << "SKIP not active hit" << std::endl;
        continue;
      }
      chisqold += deltaChiNew * deltaChiNew;
 
      for ( i = 0; i < npar; ++i )
      {
        diffsum[i] += ( *ideriv )[i] * deltaChiNew;
        for ( int j = 0; j < i + 1; ++j )
        { vparam.fast( i + 1, j + 1 ) += ( *ideriv )[i] * ( *ideriv )[j]; }
      }
    } // end loop over hits
 
    //****  Calculate new paramters
    int ierr;
    vparam.invert( ierr );
    if ( ierr )
    {
      if ( m_debug )
      {
        cout << "ErrMsg(warning) TrkHelixFitter:"
             << "Matrix inversion failed " << endl;
      }
      status.setFailure( 12, "Matrix inversion failed in TrkHelixFitter" );
      // break;
    }
    delpar = vparam * ( -diffsum );
    if ( m_debug ) { cout << " delpar = " << delpar << endl; }
    // The following test relies on having a fixed location for phi0 in
    //   all simple params; it should be made robust somehow!!!
    if ( fabs( delpar[1] ) > 1. )
    {
      if ( m_debug )
      {
        cout << "ErrMsg(warning) TrkHelixFitter:"
             << "Pathological  fit " << endl;
      }
      status.setFailure( 13, "Pathological fit in TrkHelixFitter." );
      // break;
    }
 
    for ( i = 0; i < npar; ++i ) params.parameter()[i] += delpar[i];
    if ( m_debug ) { cout << " params " << params.parameter() << endl; }
 
    //***** Loop through the hits again, calculating the approx change
    //   in residuals and chisq., and picking which hits should be active
    //      for next iteration.
 
    chisqnew                              = 0.0;
    lPicked                               = 0;
    double                      bigDelChi = 0.0;
    TrkHotList::nc_hot_iterator bigHit    = hitlist.end();
 
    mustIterate =
        ( mustIterate || ( iter <= 2 && lPickHits ) ); // iterate until hit-dropping allowed
    ideriv  = deriv.begin();
    idelChi = delChi.begin();
    for ( TrkHotList::nc_hot_iterator ihit = hitlist.begin(); ihit != hitlist.end();
          ++ihit, ++ideriv, ++idelChi )
    {
      if ( m_debug ) { ihit->print( std::cout ); }
      if ( !ihit->isUsable() )
      {
        if ( m_debug ) { std::cout << "hit NOT usable " << std::endl; }
        continue;
      }
      // double weight = ihit->weight(); // FIXME: why isn't weight used???
      for ( i = 0; i < npar; i++ ) { *idelChi += ( *ideriv )[i] * delpar[i]; }
      if ( ihit->isActive() ) chisqnew += *idelChi * *idelChi;
 
      // Hit-picking
      if ( !mustIterate && lPickHits )
      {
        double abDelChi = fabs( *idelChi );
        // yzhang fix nSigmaCut for each layers 2010-04-13
        if ( abDelChi <= nSigmaCut[ihit->layerNumber()] )
        {
          if ( m_debug )
          {
            std::cout << "abDelChi " << abDelChi << "<?" << nSigmaCut[ihit->layerNumber()]
                      << std::endl; // yzhang debug
          }
          if ( ihit->isActive() == 0 )
          {
            ihit->setActivity( 1 ); // reactivate hit
            if ( m_debug ) { cout << "set ACTIVE, Added " << endl; }
            lPicked = 1;
            nActive++;
            if ( ihit->whatView() == TrkEnums::xyView ) nXY++;
            else if ( ihit->whatView() == TrkEnums::zView ) nZ++;
            else if ( ihit->whatView() == TrkEnums::bothView )
            {
              nZ++;
              nXY++;
            }
          }
        }
        else
        {
          if ( ihit->isActive() )
          {
            if ( abDelChi > bigDelChi )
            {
              if ( m_debug )
              {
                std::cout << "bigest set INACTIVE, abDelChi = " << abDelChi << ">"
                          << nSigmaCut[ihit->layerNumber()] << " bigDelChi=" << bigDelChi
                          << std::endl;
              }
              bigDelChi = abDelChi;
              bigHit    = ihit;
            }
          }
        }
      } // end if iter > 2 (hit-picking)
    }   // end loop over hits
 
    // Drop hit with worst residual
    if ( lPickHits )
    {
      int lDrop = 0;
      if ( bigHit != hitlist.end() && ( nActive > minAct ) )
      {
        if ( bigHit->whatView() == TrkEnums::xyView && nXY > minXY )
        {
          nXY--;
          lDrop = 1;
        }
        else if ( bigHit->whatView() == TrkEnums::zView && nZ > minZ )
        {
          nZ--;
          lDrop = 1;
        }
        else if ( bigHit->whatView() == TrkEnums::bothView && nZ > minZ && nXY > minXY )
        {
          nZ--;
          nXY--;
          lDrop = 1;
        }
        if ( lDrop == 1 )
        {
          lPicked = 1;
          nActive--;
          bigHit->setActivity( 0 ); // deactivate hit
          if ( m_debug )
          {
            std::cout << "---deactivate hit!! delChi2=" << bigDelChi << std::endl;
            std::cout << "---";
            bigHit->print( std::cout );
            std::cout << "--------------------!! " << std::endl;
          }
        }
      }
    } // end if lPickHits
 
    /* Test for convergence. */
    chichange = chisqold - chisqnew;
    if ( m_debug ) { cout << "chisq from " << chisqold << " -> " << chisqnew << endl; }
    if ( chichange < -0.5 && !mustIterate && lPicked == 0 )
    {
      if ( m_debug )
      {
        cout << "ErrMsg(warning)"
             << " blowing up: " << chichange << endl;
      }
      /* It's blowing up. */
      setLastChisq( chisqnew );
      status.setFailure( 1 );
 
      if ( m_debug ) std::cout << "failure 1 " << std::endl;
      break;
    }
    else if ( chichange < chitest && !mustIterate && lPicked == 0 )
    {
      // We converged.
      status.setSuccess( 1 );
      setLastChisq( chisqnew );
      if ( m_debug ) std::cout << "success 1 " << std::endl;
      break;
    }
 
    if ( iter == itermax )
    {
      setLastChisq( chisqnew );
      status.setSuccess( 2 );
      if ( m_debug ) std::cout << "success 2 " << std::endl;
    }
  } /* end iteration loop */
 
  // store the error matrix
  params.covariance() = vparam;
 
  // Attempt to calculate deltaChisq for this hit (compared to leaving hit
  //     out of the fit).
  /*  chisqnew = 0;
      if (status.success()) {
      HepVector deltaAlpha(npar);
      for (ihit = 0; ihit < nhits; ihit++) {
      thisHot = hitlist(ihit);
      if (!thisHot->isActive()) continue;
      HepVector derivs(npar, 0);
      for (i = 0; i < npar; i++) {
      derivs[i] = deriv[ihit][i];
      }
      double weight = thisHot->weight();
      double resid = thisHot->resid();
      deltaAlpha = vparam * derivs;
      deltaAlpha *= (resid * weight);
  //      cout  << resid * resid * weight
  //           << "  " << resid * weight * dot(derivs, deltaAlpha) << endl;
  //      thisHot->chi2Contrib = -dot(deltaAlpha, temp) + resid * resid * weight
  //        + 2. * resid * weight * dot(derivs, deltaAlpha);
  }
  }
  */
 
  // Change reference point back to origin
  // if (shiftRef) {
  //    Point3D.home(0.,0.,0.);
  //    theTraj.changePoint(home, fltOffset);
  //    hitlist[0]->parentTrack()->resetT0(oldT0);
  // for (ihit = 0; ihit < nhits; ihit++) {
  //      thisHot = hitlist[ihit];
  //      thisHot->setFltLen( thisHot->fltLen() - fltOffset );
  // }
  //}
  return status;
}