TrkFitter-00-01-13/src/TrkHelixFitter.cxx

Go to the documentation of this file.

//--------------------------------------------------------------------------
// File and Version Information:
//      $Id: TrkHelixFitter.cxx,v 1.1.1.1 2017/12/15 12:01:44 huangzhen 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 <assert.h>
#include "TrkFitter/TrkHelixFitter.h"
#include "TrkBase/TrkSimpTraj.h"
#include "TrkBase/TrkParams.h"
#include "TrkBase/TrkHitOnTrk.h"
#include "CLHEP/Matrix/Vector.h" 
#include "CLHEP/Geometry/Point3D.h"
#include "CLHEP/Matrix/Matrix.h"
#include "CLHEP/Matrix/SymMatrix.h"
#include "TrkBase/TrkEnums.h"
#include "TrkBase/TrkErrCode.h"
#include "TrkBase/TrkHotList.h"
#include "CgemData/CgemHitOnTrack.h"
#include <vector>
#include <iomanip>
using std::cout;
using std::endl;
 
bool TrkHelixFitter::m_debug = false;
//bool TrkHelixFitter::m_debug = true;
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);
    //}    
 
    int ncluster(0);
    for (TrkHotList::nc_hot_iterator ihit = hitlist.begin(); ihit != hitlist.end(); ++ihit) {
        if(typeid(*ihit)==typeid(CgemHitOnTrack))ncluster++;
    }  //end loop over hits
    //*** Things that don't change with each iteration 
    int nhits = hitlist.nHit();
    std::vector<double> delChi(nhits,0);
    std::vector<double> XdelChi(ncluster,0);
    std::vector<double> VdelChi(ncluster,0);
    std::vector<std::vector<double> > deriv(nhits);
    std::vector<std::vector<double> > Xderiv(ncluster);
    std::vector<std::vector<double> > Vderiv(ncluster);
 
    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<std::vector<double> >::iterator iXderiv = Xderiv.begin();
    std::vector<std::vector<double> >::iterator iVderiv = Vderiv.begin();
    std::vector<double>::iterator idelChi = delChi.begin();
    std::vector<double>::iterator iXdelChi = XdelChi.begin();
    std::vector<double>::iterator iVdelChi = VdelChi.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++;
            }
        }
 
        if(typeid(*ihit)==typeid(CgemHitOnTrack)){
            iXderiv->resize(npar);
            iVderiv->resize(npar);
            iXderiv++;
            iVderiv++;
        }
        // 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 Xderivs(npar);//zhang change
    HepVector Vderivs(npar);//zhang change
    //  HepVector derivs(npar);//yzhang temp
    TrkErrCode calcResult;
    //**** Iterate fit. 
    size_t itermax = 12;
    for (size_t iter = 1; iter <= itermax; iter++) {
        if(m_debug)cout<<"iter->"<<iter<<endl;
        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<std::vector<double> >::iterator iXderiv = Xderiv.begin();
        std::vector<std::vector<double> >::iterator iVderiv = Vderiv.begin();
        std::vector<double>::iterator idelChi = delChi.begin();
        std::vector<double>::iterator iXdelChi = XdelChi.begin();
        std::vector<double>::iterator iVdelChi = VdelChi.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;
            double XdeltaChiNew;
            double VdeltaChiNew;
            if (calcResult.success()) {
                if (iter < 2) { // FIXME? only update derivatives at first iteration...
                    if(typeid(*ihit)==typeid(CgemHitOnTrack)){
                        CgemHitOnTrack* cgemHot = dynamic_cast<CgemHitOnTrack*> (ihit.get());
                        calcResult = cgemHot->getFitStuff(Xderivs, XdeltaChiNew, Vderivs, VdeltaChiNew);
                        for (i=0; i<npar; ++i){
                            (*iXderiv)[i] = Xderivs[i];
                            (*iVderiv)[i] = Vderivs[i];
                        }
                    }else{
                        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  {
                    if(typeid(*ihit)==typeid(CgemHitOnTrack)){
                        CgemHitOnTrack* cgemHot = dynamic_cast<CgemHitOnTrack*> (ihit.get());
                        calcResult = cgemHot->getFitStuff(XdeltaChiNew, VdeltaChiNew);
                    }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
                if(typeid(*ihit)==typeid(CgemHitOnTrack)){
                    iXderiv++;
                    iVderiv++;
                    iXdelChi++;
                    iVdelChi++;
                }
                continue;
            }
            mustIterate = (mustIterate || (calcResult.success() != 1));
            if(typeid(*ihit)==typeid(CgemHitOnTrack)){
                *iXdelChi = XdeltaChiNew;
                *iVdelChi = VdeltaChiNew;
            }else{
                *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;
                if(typeid(*ihit)==typeid(CgemHitOnTrack)){
                    iXderiv++;
                    iVderiv++;
                    iXdelChi++;
                    iVdelChi++;
                }
                continue;
            }
            if(typeid(*ihit)==typeid(CgemHitOnTrack)){
                chisqold += XdeltaChiNew * XdeltaChiNew;
                chisqold += VdeltaChiNew * VdeltaChiNew;
                for (i = 0; i < npar; ++i) {
                    diffsum[i] += (*iXderiv)[i] * XdeltaChiNew;
                    diffsum[i] += (*iVderiv)[i] * VdeltaChiNew;
                    for (int j = 0; j < i+1; ++j) {
                        vparam.fast(i+1,j+1) += (*iXderiv)[i] * (*iXderiv)[j];
                        vparam.fast(i+1,j+1) += (*iVderiv)[i] * (*iVderiv)[j];
                    }
                }
            }else{
                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];
                    }
                }
            }
            if(typeid(*ihit)==typeid(CgemHitOnTrack)){
                iXderiv++;
                iVderiv++;
                iXdelChi++;
                iVdelChi++;
            }
            if (iter < 0) {
                if(typeid(*ihit)==typeid(CgemHitOnTrack)){
                    cout<<ihit->layerNumber()<<"  "<<setw(12)<<XdeltaChiNew<<" || "<<setw(12)<<(Xderivs)[0]<<"  "<<setw(12)<<(Xderivs)[1]<<"  "<<setw(12)<<(Xderivs)[2]<<"  "<<setw(12)<<(Xderivs)[3]<<"  "<<setw(12)<<(Xderivs)[4]<<endl;
                    cout<<ihit->layerNumber()<<"  "<<setw(12)<<VdeltaChiNew<<" || "<<setw(12)<<(Vderivs)[0]<<"  "<<setw(12)<<(Vderivs)[1]<<"  "<<setw(12)<<(Vderivs)[2]<<"  "<<setw(12)<<(Vderivs)[3]<<"  "<<setw(12)<<(Vderivs)[4]<<endl;
                }else cout<<ihit->layerNumber()<<"  "<<setw(12)<<deltaChiNew<<" || "<<setw(12)<<(*ideriv)[0]<<"  "<<setw(12)<<(*ideriv)[1]<<"  "<<setw(12)<<(*ideriv)[2]<<"  "<<setw(12)<<(*ideriv)[3]<<"  "<<setw(12)<<(*ideriv)[4]<<endl;
            }
        }  // 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();
        iXderiv = Xderiv.begin();
        iVderiv = Vderiv.begin();
        idelChi = delChi.begin();
        iXdelChi = XdelChi.begin();
        iVdelChi = VdelChi.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; }
                if(typeid(*ihit)==typeid(CgemHitOnTrack)){
                    iXderiv++;
                    iVderiv++;
                    iXdelChi++;
                    iVdelChi++;
                }
                continue;
            }
            //double weight = ihit->weight(); // FIXME: why isn't weight used???
            if(typeid(*ihit)==typeid(CgemHitOnTrack)){
                for (i = 0; i < npar; i++) {
                    *iXdelChi += (*iXderiv)[i] * delpar[i];
                    *iVdelChi += (*iVderiv)[i] * delpar[i];
                }
                if (ihit->isActive()){
                    chisqnew += *iXdelChi * *iXdelChi;
                    chisqnew += *iVdelChi * *iVdelChi;
                }
            }else{
                for (i = 0; i < npar; i++) {
                    *idelChi += (*ideriv)[i] * delpar[i];
                }
                if (ihit->isActive()) chisqnew += *idelChi * *idelChi;
            }
            // Hit-picking
            if (!mustIterate && lPickHits) {
                double abDelChi;
                if(typeid(*ihit)==typeid(CgemHitOnTrack)) abDelChi = fabs(*iXdelChi + *iVdelChi)/2;//FIXME
                else 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)
            if(typeid(*ihit)==typeid(CgemHitOnTrack)){
                iXderiv++;
                iVderiv++;
                iXdelChi++;
                iVdelChi++;
            }
        } //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;
}