Example #1
0
def main():
    nametag = os.path.splitext( os.path.basename(args.file) )[0]
    if args.name:
        nametag = nametag + "-" + args.name

    Chi2Sum = 0.
    NdfSum  = 0.
    LostSum  = 0.
    nTracks = 0

    start = time.clock()

    np.random.seed(47117)

    # Open binary file
    binaryFile = None
    if args.save:
        binaryFileName = "milleBinaryISN" + "_" + nametag
        binaryFile = open("%s.dat" % binaryFileName, "wb")
    
    # Open input file
    events = hpseventst.readHPSEvents(args.file, args.nevents, args.ntracks)
    
    print 'Read %d events from file' % len(events)

    plotsTopBot = hps_plots.plotter(nametag,'pdf',False,True,True, False)
    plotsTop = hps_plots.plotter(nametag,'pdf',False,True,False, False)
    plotsBot = hps_plots.plotter(nametag,'pdf',False,False,True, False)


    # loop over all events
    for event in events:

        if args.ntracks > 0 and nTracks > args.ntracks:
            break

        if args.debug or nTracks % 1000 == 0:
            print 'Processed %d tracks, now at event id %d with %d tracks ' % (nTracks, event.id, len(event.tracks))


        # loop over all tracks in the event
        for track in event.tracks:

            if args.debug:
                print 'track %d in event %d has %d strips' % (track.id, event.id,len(track.strips))

            # create the GBL trajectory
            traj = GblTrajectory(False)
            
            # point-to-point Jacobian
            jacPointToPoint = np.eye(5)

            # store projections for later use
            proL2m_list = {} 

            # start trajectory at reference point
            # this defines zero path length (s=0)
            point = GblPoint(jacPointToPoint)
            iLabelRef = traj.addPoint(point)

            # save mapping between label and strip object 
            stripLabelMap = {}

            # track direction in global frame
            tDirGlobal = track.direction()

            sinLambda = math.sin( track.clPar[1] )
            sinPhi = math.sin( track.clPar[2] )
            cosLambda = math.sqrt(1 - sinLambda ** 2)
            cosPhi = math.sqrt(1 - sinPhi ** 2)

            if args.debug:
                print 'tDir', tDirGlobal
                print 'lambda ', track.clPar[1], ' phi ', track.clPar[2]
                print 'sinLambda ', sinLambda, ' sinPhi ', sinPhi
            

            # path length
            s = 0.


            # loop over all strip clusters on the track
            for strip in track.strips:
                
                if args.debug:
                    print 'Processing strip id %d, millepedeId %d on sensor %s ' % (strip.id, strip.millepedeId,strip.deName)

                # calculate step from previous point
                step = strip.pathLen - s

                if args.debug:
                    print 'step ', step , '     (path length ',strip.pathLen, ' s ', s, ')'
                

                # Find the projection from tracking to measurement frame
                mDir = np.array( [strip.u, strip.v] )

                if args.debug:
                    print 'u: ',strip.u
                    print 'v: ',strip.v
                    print 'w: ',strip.w
                    print 'mDir:\n', mDir

                # Find the projection from curvilinear to measurement frame

                # Track direction in the curvilinear frame (U,V,T)
                # U = Z x T / |Z x T|, V = T x U
                uvDir = np.array([[-sinPhi, cosPhi, 0.], \
                                    [-sinLambda * cosPhi, -sinLambda * sinPhi, cosLambda]])


                if args.debug:
                    print 'Track direction in curvilinear frame\n',uvDir

                # projection from  measurement to local (curvilinear uv) directions (duv/dm)
                proM2l = np.dot(uvDir, mDir.T)

                # projection from local (uv) to measurement directions (dm/duv)
                proL2m = np.linalg.inv(proM2l)
                proL2m_list[strip.id] = proL2m

                if args.debug: 
                  print 'proM2l:\n', proM2l
                  print 'proL2m:\n', proL2m


                    
                # first get the projection from curvilinear to XYZ (or "global") frame
                #prjGlobalToCl = track.perToClPrj
                #prjClToGlobal = np.linalg.inv(prjGlobalToCl)

                #if args.debug:
                #    print 'prjGlobalToCl\n',prjGlobalToCl
                #    print 'prjClToGlobal\n',prjClToGlobal

                # now get the projection from global frame to measurement (or "sensor") frame
                # in HPS we call the measurement frame the "local sensor frame" and denote by "uvw"
                #prjGlobalToMeas = np.array( [strip.u, strip.v, strip.w] )
            
                #if args.debug:
                #    print 'prjGlobalToMeas\n', prjGlobalToMeas
                #    print 'prjGlobalToMeas.T\n', prjGlobalToMeas.T

                # now calculate the projection of the curvilinear to measurement frame
                # this is the so-called local to measurement transformation
                #prjClToMeas = np.dot( prjClToGlobal, prjGlobalToMeas.T) 
                #prjMeasToCl = np.linalg.inv( prjClToMeas )
                # adjust dimension of the projection, track direction coordinate is always 0
                #proL2m = prjClToMeas[:2,:2]        
                
                #if args.debug:
                #    print 'prjClToMeas\n', prjClToMeas
                #    #print 'prjMeasToCl\n', prjMeasToCl

                # residual and errors in measurement frame
                meas = np.array( [strip.ures, 0.] )
                measErr = np.array( [strip.ures_err, strip.ures_err] )
                measPrec = 1.0 / measErr ** 2
                measPrec[1] = 0. # no weight for measurement along the strip direction

                # Find the Jacobian to be able to propagate the covariance matrix to this strip position
                # "cosLambda" is the projection from the difference views
                # note that for this Jacobian the cosLambda only enters if there is a B-field
                jacPointToPoint = utils.gblSimpleJacobian(step, cosLambda, 0.)

                # Create a GBL point
                point = GblPoint(jacPointToPoint)


                if args.debug: 
                    print 'meas ', meas, ' measErr ', measErr, ' measPrec ', measPrec
                    print 'jacPointToPoint\n', jacPointToPoint
                    print 'proL2m \n', proL2m

                # Add a measurement to the point                
                point.addMeasurement([proL2m, meas, measPrec])

                # Add scatterer in curvilinear frame to the point
                # no direction in this frame
                scat = np.array([0., 0.])
                # Scattering angle in the curvilinear frame
                scatErr = np.array([ strip.scatAngle, strip.scatAngle / cosLambda]) 
                scatPrec = 1.0 / scatErr ** 2

                if args.debug: 
                    print 'scatPrec ', scatPrec, ' scatErr ', scatErr, ' cosLambda ', cosLambda
                
                point.addScatterer( [scat, scatPrec] )

                # Calculate global derivatives for this point
                # needs a few vectors in measurement frame

                # Projection matrix from tracking frame to measurement frame
                # t_u = dot(u,i)*t_i + dot(u,j)*t_j + dot(u,k)*t_k
                # where t_u is component in new u direction and t_i is in the i direction
                prjTrkToMeas = np.array([strip.u,strip.v,strip.w])
                # rotate to track direction to measurement frame          
                tDirMeas = np.dot( prjTrkToMeas, tDirGlobal.T) 
                normalMeas = np.dot( prjTrkToMeas, np.array([strip.w]).T )
                # vector coplanar with measurement plane from origin to prediction
                tDiff = np.array( [strip.tPos]) - np.array( [strip.origin] )
                # rotate to measurement frame          
                tPosMeas = np.dot( prjTrkToMeas, tDiff.T) 

                if args.debug: 
                    print 'tDirGlobal ', tDirGlobal
                    print 'rotation matrix to meas frame\n', prjTrkToMeas
                    print 'tPosGlobal ', np.array( [strip.tPos]) , ' (origin ', np.array( [strip.origin] ),')'
                    print 'tDiff ', tDiff
                    print 'tPosMeas ', tPosMeas
                    print 'normalMeas ', normalMeas

                # non-measured coordinates
                vmeas = 0.
                wmeas = 0.

                # actually calculate the derivatives
                glDers = utils.globalDers(strip.millepedeId, strip.meas, vmeas, wmeas, tDirMeas, tPosMeas, normalMeas)
                
                if args.debug:
                    glDers.dump()

                # restructure to two arrays to fit interface
                ders = glDers.getDers( track.isTop() )
                labGlobal = ders['labels']
                addDer = ders['ders']
                if args.debug or (1==1 and \
                                  not track.isTop() and \
                                  getAxialStereo(strip.deName) == 'stereo' and \
                                  math.tan( track.clPar[1] ) > -0.011 and \
                                  math.tan( track.clPar[1] ) < -0.01 and \
                                  track.clPar[2] >0.01 and \
                                  track.clPar[2] <0.011):
                    
                    print '=== Global derivatives ==='
                    tanLambda = math.tan ( track.clPar[1] )
                    phi0 = track.clPar[2]
                    print 'tanLambda ', tanLambda, ' phi0 ', phi0
                    print 'track dir tracking frame     ', tDirGlobal
                    print 'track dir measurement frame  ', tDirMeas
                    print 'track pred tracking frame    ', strip.tPos
                    print 'track pred measurement frame ', tPosMeas
                    print 'deName ', strip.deName
                    print 'normalMeas ', normalMeas
                    print 'strip.u ', strip.u
                    print 'strip.ures ', strip.ures
                    print 'global derivatives for ', strip.deName, ' with id ', strip.id, ' and millepede id ', strip.millepedeId
                    print labGlobal.shape
                    for ider in range(labGlobal.shape[1]):
                        print '%7d %10.3e  %s' % (labGlobal[0][ider], addDer[0][ider], strip.deName)
                    print '====================== ==='

                # actually add the global derivatives to the point
                point.addGlobals(labGlobal, addDer)

                # add point to trajectory
                iLabel = traj.addPoint(point)
                
                # save strip and label map
                stripLabelMap[strip] = iLabel
            
                # go to next point
                s += step

                if args.debug:
                    print 'Done processing strip %d for track %d in event %d' % (strip.id, track.id, event.id)

            if args.debug:
                print 'Done adding points to trjacetory for track %d in event %d' % (track.id, event.id)


            if args.debug:
                print 'Do the fit'

            Chi2, Ndf, Lost = traj.fit()


            #if utils.chi2Prob(Chi2,Ndf) < 0.1:
            #    continue

            if args.save:
                traj.milleOut( binaryFile )

            # sum up fits
            Chi2Sum += Chi2
            NdfSum += Ndf
            LostSum += Lost            

            # get corrections and covariance matrix at points; collect the result in one object
            result = hpseventst.GBLTrajectory(track,traj)

            if nTracks < 2 or args.debug:
                print 'fit result: Chi2=%f Ndf=%d Lost=%d' % (Chi2, Ndf, Lost)
                result.dump()
            



            # loop over the two halves and the combined to get all plots
            
            for iplot in range(3):
                if iplot == 0:
                    plots = plotsTopBot
                elif iplot == 1 and track.isTop():
                    plots = plotsTop
                elif iplot == 2 and not track.isTop():
                    plots = plotsBot
                else:
                    continue

                plots.h_chi2.Fill(Chi2)
                plots.h_chi2ndf.Fill(Chi2/Ndf)
                plots.h_chi2prob.Fill(utils.chi2Prob(Chi2,Ndf))

                # loop over all the strips
                for strip in track.strips:
                    if strip not in stripLabelMap:
                        raise HpsGblException('this strip is not in the label map?!')
                    iLabel = stripLabelMap[strip]
                    point = iLabel

                    #residual for initial fit
                    plots.fillSensorPlots("res", strip.deName, strip.ures)

                    locPar, locCov = result.traj.getResults(point)
                    kinkLambda = result.kink(point,result.idx_lambda)
                    kinkPhi = result.kink(point,result.idx_phi)

                    plots.fillSensorPlots("corr_lambda",strip.deName, locPar[result.idx_lambda])
                    plots.fillSensorPlots("corrdiff_lambda",strip.deName, kinkLambda)

                    plots.fillSensorPlots("corr_phi",strip.deName, locPar[result.idx_phi])
                    plots.fillSensorPlots("corrdiff_phi",strip.deName, kinkPhi)

                    # correction to xT,yT from GBL fit
                    plots.fillSensorPlots("xTcorr",strip.deName, locPar[result.idx_xT])
                    plots.fillSensorPlots("yTcorr",strip.deName, locPar[result.idx_yT])

                    corr = np.matrix( [ locPar[result.idx_xT], locPar[result.idx_yT] ] )

                    # project to measurement direction
                    corr_meas = np.matrix( proL2m_list[strip.id] ) * np.transpose( np.matrix( corr ) )
                    ures_gbl = strip.ures - corr_meas[0,0] # note minus sign due to definition of residual
                    plots.fillSensorPlots("res_gbl", strip.deName, ures_gbl)
                    plots.fillSensorPlots("res_gbl_vs_vpred", strip.deName, [ures_gbl,tPosMeas[1]])
                    if abs(strip.meas) > 20.:
                        raise HpsGblException('really, this shouldnt happen? meas= ' + str(strip.meas))
                    plots.fillSensorPlots("res_gbl_vs_u", strip.deName, [ures_gbl, strip.meas] )


            nTracks += 1  

            if args.debug:
                print 'Done processing track %d in event %d' % (track.id, event.id)

    
    if binaryFile != None:
        if not binaryFile.closed:
            binaryFile.close()

    end = time.clock()
    print " Processed %d tracks " % nTracks
    print " Time [s] ", end - start
    if nTracks > 0:
        print " Chi2Sum/NdfSum ", Chi2Sum / NdfSum
        print " LostSum/nTracks ", LostSum / nTracks
        plotsTopBot.show(args.save,args.nopause)
        plotsTop.show(args.save,args.nopause)
        plotsBot.show(args.save,args.nopause)
        if args.save:
            hps_plots.saveHistosToFile(gDirectory,'gbltst-hps-plots-%s.root' % nametag)
        
    else:
        print 'No tracks processed'
Example #2
0
def exampleHpsTest(inputfile):
  '''
  Read initial fit and points from  test file
  Create trajectory from points,
  fit and write trajectory to MP-II binary file,
  get track parameter corrections and covariance matrix at points.
  
  Detector arrangement from text file
  '''  

  Chi2Sum = 0.
  NdfSum = 0
  LostSum = 0.

  Bz = -0.5 # full detector  -0.491 for test run detector
  bfac = 0.0002998 * Bz # for Bz in Tesla, momentum in GeV and Radius in mm

  np.random.seed(47117)

  binaryFile = open("milleBinaryISN.dat", "wb")

  inputFile = open(inputfile, 'r')
  events = utils.readHPSEvents(inputFile, nEventsMax)
  
  print 'Read %d events from file' % len(events)

  
  #print " GblHpsTest $Rev: 234 $ ", nTry, nLayer
  nTry = 0
  start = time.clock()

  h_chi2prob_gbl_truth = TH1F('h_chi2prob_gbl_truth','h_chi2prob_gbl_truth',50,0,1)
  h_chi2prob_initial_truth = TH1F('h_chi2prob_initial_truth','h_chi2prob_initial_truth',50,0,1)


  for event in events:
    
    if debug: print '\nEvent %d has %d tracks ' % (event.id, len(event.tracks))
    

    for track in event.tracks:

      if debug: print '\nProcessing track %d \n p = %.3f p(truth)=%.3f' % (track.id, track.p(bfac), track.p_truth(bfac))

      # if there's no truth info -> skip it
      if track.p_truth(bfac) == 0.:
        print 'No truth info, skip track %d in event %d ' % (track.id, event.id)
        continue
      
      traj = GblTrajectory(True)

      #print " perPar ", track.perParTruth
#      hlxPar = [ cmp(bfac, 0.) * track.curvature(), track.phi0(), track.d0(), track.slope(), track.z0()]
      hlxPar = [ cmp(bfac, 0.) * track.curvature(), track.phi0(), -track.d0(), track.slope(), track.z0()]
      #hlxPar = [ cmp(bfac, 0.) * track.curvature_truth(), track.phi0_truth(), track.d0_truth(), track.slope_truth(), track.z0_truth()]
      #print " hlxPar ", hlxPar
      hlx = SimpleHelix(hlxPar)
      cosLambda = hlx.getZSDirection()[0]
      # stupid error for now
      #clCov = np.eye(5)
      #for i in range(5):
      #  clCov[i, i] = clErr[i] ** 2
      #clCov = track.clCov

      stripLabelMap = {}
      
      if debug: print 'Track has %d strip clusters' % len(track.strips)
      # arc length
      s = 0.
      # point-to-point jacobian (from previous point)    
      jacPointToPoint = np.eye(5)
      #start trajectory at reference point (defining s=0)
      point = GblPoint(jacPointToPoint)
      refLabel = traj.addPoint(point)

      # multiple scattering covariance matrix (for curvilinear track parameters)        
      msCov = np.zeros((5, 5))

      # store projection for later use
      proM2l_list = {} 
      proL2m_list = {} 

      for strip in track.strips:
        
        if debug: print '\nProcessing strip %d at layer %d ' % (strip.id, strip.layer)
 
        # direction in detector plane in XY: (xDir, yDir, 0.) 
        xDir = -strip.w[1]
        yDir = strip.w[0]
        # direction in detector plane in Z: (0., 0., 1.) 
        # position of detector (center)
        xDet = strip.origin[0]
        yDet = strip.origin[1]
        zDet = strip.origin[2]
        # get prediction along the 2 directions (in the detector plane)
        pred = hlx.getExpectedPlanePos(xDet, yDet, xDir, yDir, zDet)
        if pred is None:
          continue # no intersection of track and detector
        # prediction position (in global system)
        xPred = xDet + pred[0] * xDir
        yPred = yDet + pred[0] * yDir
        zPred = zDet + pred[1]

        # project onto u-direction
        uPred = pred[0] * (xDir * strip.u[0] + yDir * strip.u[1]) + pred[1] * strip.u[2]

        # stupid iterative intercept
        predIter = utils.getXPlanePositionIterative(track.perPar,strip.origin,strip.w,1.0e-8)
        diffIter = predIter - strip.origin
        uPredIter = np.dot(strip.u , diffIter.T )
        
        #xPred = predIter[0]
        #yPred = predIter[1]
        #zPred = predIter[2]
        #pred0 = (predIter[0] - xDet) / xDir
        #pred1 = (predIter[1] - zDet)
        #uPredIter = pred0 * (xDir * strip.u[0] + yDir * strip.u[1]) + pred1 * strip.u[2]
        

        # u residuum
        uRes = strip.meas - uPred
        uResIter = strip.meas - uPredIter
        #uRes = uResIter
        # (3D) arc-length
        sArc = pred[3] / cosLambda
        phi = pred[4]
        #print " pred ", sArc, xPred, yPred, zPred
        if nTry == 0:
          print " uRes ",strip.id, ' uRes ',  uRes, ' pred ', xPred, yPred, zPred, ' s(3D) ', sArc
          #print " uRes ", strip.id, 'uRes(Cl) ', uRes, ' uRes ', strip.ures, ' uResIter ', uResIter, ' pred ', xPred, yPred, zPred, ' predIter ', predIter, ' s(3D) ', sArc
         #print -1*track.d0(),track.z0(),track.phi0(),track.slope(),track.curvature()
          #print predIter, diffIter, strip.u, uPredIter, strip.meas
          #print " uRes ", strip.id, uRes, uResIter, strip.ures, strip.ures_err, ' pred ', xPred, yPred, zPred, ' s ', pred[3], ' s3D ', sArc, ' on plane ', np.dot(np.array([[xPred, yPred, zPred]]) - strip.origin , np.array([strip.w]).T )
          #print ' predIter ', predIter
          #print ' predIter diff ', (np.array([xPred,yPred,zPred]) - predIter) 

          #print " java ", strip.id , strip.ures, ' pred ', strip.tPos, ' on plane ', np.dot(np.array([strip.tPos]) - strip.origin , np.array([strip.w]).T ) 
        
        step = sArc - s

        if debug: print 'Step %f (s %f pathLen %f)' % (step, s, sArc)

        # measurement direction(s): (m[0]=u, m[1]=v) 
        if debug: print 'Strip udir', strip.u
        if debug: print 'Strip vdir', strip.v
        
        mDir = np.array([strip.u, strip.v])
        
        if debug: 
          print 'mDir:\n', mDir
        
        # track direction: in x directon
        sinLambda = strip.sinLambda
        cosLambda = math.sqrt(1.0 - sinLambda ** 2)
        sinPhi = strip.sinPhi
        cosPhi = math.sqrt(1.0 - sinPhi ** 2)
        
        if debug: print 'Track direction sinLambda=%f sinPhi=%f' % (sinLambda, sinPhi)
        
        #  tDir = np.array([cosLambda * cosPhi, cosLambda * sinPhi, sinLambda])
        # U = Z x T / |Z x T|, V = T x U
        uvDir = np.array([[-sinPhi, cosPhi, 0.], \
                            [-sinLambda * cosPhi, -sinLambda * sinPhi, cosLambda]])
        
        
        # projection measurement to local (curvilinear uv) directions (duv/dm)
        proM2l = np.dot(uvDir, mDir.T)

        proM2l_list[strip.id] = proM2l

        if debug: print 'proM2l:\n', proM2l

        # projection local (uv) to measurement directions (dm/duv)
        proL2m = np.linalg.inv(proM2l)

        proL2m_list[strip.id] = proL2m

        if debug: print 'proL2m:\n', proL2m

        # measurement/residual in the measurement system
        #meas = np.array([strip.ures, 0.])
        meas = np.array([uRes, 0.])
        #meas[0] += deltaU[iLayer] # misalignment
        measErr = np.array([strip.ures_err, strip.ures_err])
        measPrec = 1.0 / measErr ** 2
        measPrec[1] = 0. # 1D measurement perpendicular to strip direction
        
        if debug: print 'meas ', meas, ' measErr ', measErr, ' measPrec ', measPrec

        #propagate to this strip
        #jacPointToPoint = utils.gblSimpleJacobianLambdaPhi(step, cosLambda, bfac)
        jacPointToPoint = hlx.getPropagatorSimple(step, abs(bfac))
        #print jacPointToPoint
        
        point = GblPoint(jacPointToPoint)
        
        if debug: 
          print 'jacPointToPoint to extrapolate to this point:'
          print point.getP2pJacobian()
        
        #propagate MS covariance matrix        
        msCov = np.dot(jacPointToPoint, np.dot(msCov, jacPointToPoint.T))
        # MS covariance for measurements
        measMsCov = np.dot(proL2m, np.dot(msCov[3:, 3:], proL2m.T))
 
        if debug:
          print " uPred ", strip.id, pred[3], uPred, strip.meas, strip.ures, strip.ures_err, measMsCov[0, 0]
       
        #plots.h_measMsCov.Fill(float(strip.layer),measMsCov[0,0])
        
        if debug:
          print 'msCov propagated to this point:'
          print msCov
          print 'measMsCov at this point to be used in measPrec:'
          print measMsCov
        
        if useUncorrMS:
          # blow up measurement errors according to multiple scattering
          measPrec[0] = 1.0 / (measErr[0] ** 2 + measMsCov[0, 0])
        
        point.addMeasurement([proL2m, meas, measPrec])

        if debug:
          print 'measMsCov ', measMsCov[0, 0]
        
        scat = np.array([0., 0.])
        scatErr = np.array([ strip.scatAngle, strip.scatAngle / cosLambda]) 
        scatPrec = 1.0 / scatErr ** 2
        
        if not useUncorrMS:
          point.addScatterer([scat, scatPrec])
        
        #update MS covariance matrix
        msCov[1, 1] += scatErr[0] ** 2; msCov[2, 2] += scatErr[1] ** 2

        if debug:
          print 'adding scatError to the msCov from this point:'
          print scatErr
                
        addDer = np.array([[1.0], [0.0]])
        #top or bottom half        
        if math.copysign(1, sinLambda) > 0:
          offset = 11101
        else:
          offset = 21101
        labGlobal = np.array([[offset + strip.layer], [0]])
        point.addGlobals(labGlobal, addDer)
        
        # add point to trajectory
        iLabel = traj.addPoint(point)
        s += step
        stripLabelMap[strip] = iLabel
      
      
      if debug: print 'Do the fit'
      Chi2, Ndf, Lost = traj.fit()

      # write to millepede
      traj.milleOut(binaryFile)

      # sum up    
      Chi2Sum += Chi2
      NdfSum += Ndf
      LostSum += Lost
      # get corrections and covariance matrix at points 
      result = utils.GBLResults(track)
      #traj.dump()
        
      if nTry == 0:
        print 'fit result: Chi2=%f Ndf=%d Lost=%d' % (Chi2, Ndf, Lost)
        print 'get corrections and covariance matrix for %d points:' % 1 #traj.getNumPoints()
      
      for i in range(1, traj.getNumPoints() + 1):      
        # label start at 1
        locPar, locCov = traj.getResults(-i)
        if nTry < 0:
          print " >Point ", i
          print " locPar ", locPar
          #print " locCov ", locCov      
        result.addPoint(-i, locPar, locCov)
        locPar, locCov = traj.getResults(i)
        if nTry < 0:
          print " Point> ", i
          print " locPar ", locPar
          #print " locCov ", locCov
        result.addPoint(i, locPar, locCov)
      


      # calculate the truth chi2 from initial fit
      # get the truth and fitted params with indexes same as cov matrix of initial fit (dca,phi0,curv,z0,slope)
      perParVec = np.array([track.d0(), track.phi0(), track.curvature(), track.z0(), track.slope()])
      perParVecTruth = np.array([track.d0_truth(), track.phi0_truth(), track.curvature_truth(), track.z0_truth(), track.slope_truth()])
      perParVecRes = perParVec - perParVecTruth
      chi2_initial_truth = np.dot(perParVecRes, np.dot(np.linalg.inv(track.perCov) , perParVecRes))

      # calculate the truth chi2 from gbl fit at vertex
      clParVtx = np.array(track.clPar) + np.array(result.locPar[1])
      clParTruth = np.array(track.clParTruth)
      clParRes = clParVtx - clParTruth
      chi2_gbl_truth = np.dot(clParRes, np.dot(np.linalg.inv(result.locCov[1]), clParRes))

      #print " truth ", track.clParTruth
      #print " res ", refLabel, result.locPar[refLabel], result.locCov[refLabel]
      # calculate chi2 for seeding by truth 
      label = 1#refLabel
      chi2_res = np.dot(result.locPar[label], np.dot(np.linalg.inv(result.locCov[label]), result.locPar[label]))
      #chi2_res4 = np.dot(result.locPar[label][:4], np.dot(np.linalg.inv(result.locCov[label][:4, :4]), result.locPar[label][:4]))
      print " Chi2: ",
      #for i in range(5):
      #  print track.clParTruth[i], result.locPar[label][i] / math.sqrt(result.locCov[label][i][i]),
      print event.id, chi2_res, chi2_gbl_truth, chi2_initial_truth
      #print clParRes
      #print track.clPar
      #print result.locPar[1]
      #print clParVtx
      #print clParTruth
      #print result.locCov[label]
      h_chi2prob_gbl_truth.Fill(TMath.Prob(chi2_gbl_truth,5))
      h_chi2prob_initial_truth.Fill(TMath.Prob(chi2_initial_truth,5))

      '''
      print " clPar ", track.clPar
      print " clParTruth ", track.clParTruth
      print " clParVtx ", clParVtx
      print " clParRes ", clParRes
      print " res[1] ", np.array(result.locPar[1])
      print " cov[1] ", result.locCov[1]
      '''
      '''
      # plots

      plots.h_clPar_xT.Fill(track.clPar[3])
      plots.h_clPar_yT.Fill(track.clPar[4])
      plots.h_clPar_qOverP.Fill(track.clPar[0])
      plots.h_clPar_lambda.Fill(track.clPar[1])
      # transform phi to plot nicer
      if track.clPar[2]<math.pi:
        plots.h_clPar_phi.Fill(track.clPar[2])
      else:
        plots.h_clPar_phi.Fill(track.clPar[2]-math.pi*2)
      plots.h_clPar_res_qOverP.Fill(clParRes[0,0])
      plots.h_clPar_res_lambda.Fill(clParRes[0,1])
      plots.h_clPar_res_phi.Fill(clParRes[0,2])
      plots.h_clPar_res_xT.Fill(clParRes[0,3])
      plots.h_clPar_res_yT.Fill(clParRes[0,4])

      plots.h_clPar_pull_qOverP.Fill(clParRes[0,0]/math.sqrt(result.locCov[1][0,0]))
      plots.h_clPar_pull_lambda.Fill(clParRes[0,1]/math.sqrt(result.locCov[1][1,1]))
      plots.h_clPar_pull_phi.Fill(clParRes[0,2]/math.sqrt(result.locCov[1][2,2]))
      plots.h_clPar_pull_xT.Fill(clParRes[0,3]/math.sqrt(result.locCov[1][3,3]))
      plots.h_clPar_pull_yT.Fill(clParRes[0,4]/math.sqrt(result.locCov[1][4,4]))

      plots.h_perPar_res_d0.Fill(perParVecRes[0,0])
      plots.h_perPar_res_phi0.Fill(perParVecRes[0,1])
      plots.h_perPar_res_kappa.Fill(perParVecRes[0,2])
      plots.h_perPar_res_z0.Fill(perParVecRes[0,3])
      plots.h_perPar_res_slope.Fill(perParVecRes[0,4])
      plots.h_chi2_initial.Fill(track.chi2Initial)
      plots.h_chi2ndf_initial.Fill(track.chi2Initial/track.ndfInitial)
      plots.h_chi2_initial_truth.Fill(chi2_initial_truth)
      plots.h_chi2ndf_initial_truth.Fill(chi2_initial_truth/5.0)
      plots.h_chi2prob_initial_truth.Fill(utils.chi2Prob(chi2_initial_truth,5))
      plots.h_chi2_gbl_truth.Fill(chi2_gbl_truth)
      plots.h_chi2ndf_gbl_truth.Fill(chi2_gbl_truth/5.0)
      plots.h_chi2prob_gbl_truth.Fill(utils.chi2Prob(chi2_gbl_truth,5))
      plots.h_chi2.Fill(Chi2)
      plots.h_chi2ndf.Fill(Chi2/Ndf)
      plots.h_p.Fill(track.p(bfac))
      plots.h_qOverP.Fill(track.qOverP(bfac))
      plots.h_qOverP_truth_res.Fill(track.qOverP(bfac) - track.q()/track.p_truth(bfac))
      plots.h_p_truth.Fill(track.p_truth(bfac))
      plots.h_p_truth_res.Fill(track.p(bfac)-track.p_truth(bfac))
      plots.h_qOverP_corr.Fill(result.qOverPCorr())
      plots.h_qOverP_gbl.Fill(result.qOverP_gbl(bfac))
      plots.h_qOverP_truth_res_gbl.Fill(result.qOverP_gbl(bfac) - result.track.q()/result.track.p_truth(bfac))
      plots.h_p_corr.Fill(result.pCorr(bfac))
      plots.h_p_gbl.Fill(result.p_gbl(bfac))
      plots.h_p_truth_res_gbl.Fill(result.p_gbl(bfac) - result.track.p_truth(bfac))
      
      vtx_idx = 1 # first point is at s=0 (the "vtx" is at -670mm in test run)
      plots.h_vtx_xT_corr.Fill(result.xTCorr(vtx_idx))
      plots.h_vtx_yT_corr.Fill(result.yTCorr(vtx_idx))
      plots.h_d0_corr.Fill(result.d0Corr(vtx_idx))
      plots.h_z0_corr.Fill(result.z0Corr(vtx_idx))
      plots.h_d0.Fill(track.d0())
      plots.h_z0.Fill(track.z0())
      plots.h_d0_gbl.Fill(result.d0_gbl(vtx_idx))
      plots.h_z0_gbl.Fill(result.z0_gbl(vtx_idx))

      for label,corr in result.locPar.iteritems():
        if label>0:
          lbl = 2*(label-1) + 1
        else:
          lbl = -1*2*label
        plots.h_xT_corr.Fill(lbl, corr[result.idx_xT])
        plots.h_yT_corr.Fill(lbl, corr[result.idx_yT])
      
      

      for istrip in range(len(track.strips)):
        strip = track.strips[istrip]
          # find the label, if not found it's the vertex
        if strip in stripLabelMap:
          iLabel = stripLabelMap[strip]
        else:
          iLabel = 1

        #residuals 
        plots.h_res_layer.Fill(strip.layer,strip.ures)
        # correction to xT,yT from GBL fit
        corr = np.matrix( [result.locPar[iLabel][3], result.locPar[iLabel][4] ] )
        # project to measurement direction
        corr_meas = np.matrix( proL2m_list[strip.id] ) * np.transpose( np.matrix( corr ) )
        ures_gbl = strip.ures - corr_meas[0,0] # note minus sign due to definition of residual
        plots.h_res_gbl_layer.Fill(strip.layer,ures_gbl)
        
        # make plots for a given track only
        if nTry==0:
          plots.gr_ures.SetPoint(istrip,strip.pathLen,strip.ures)
          plots.gr_ures.SetPointError(istrip,0.,strip.ures_err)
          plots.gr_ures_truth.SetPoint(istrip,strip.pathLen,strip.uresTruth) 
          plots.gr_ures_simhit.SetPoint(istrip,strip.pathLen,strip.uresSimHit) 
          meas = np.array([strip.ures, 0.])
          #locRes = np.matrix(proM2l_list[strip.id]) *  np.transpose(np.matrix(meas))
          #xT_res = locRes[0,0]
          #yT_res = locRes[1,0]
          # find corrections to xT and yT
          plots.gr_corr_ures.SetPoint(istrip, strip.pathLen, corr_meas[0,0]) #u-direction
          ures_corr =  meas - corr_meas.T
          plots.gr_ures_corr.SetPoint(istrip, strip.pathLen, ures_corr[0,0]) #u-direction
      '''     
      nTry += 1

      
  #
  end = time.clock()
  print " Processed %d tracks " % nTry
  print " Time [s] ", end - start
  print " Chi2Sum/NdfSum ", Chi2Sum / NdfSum
  print " LostSum/nTry ", LostSum / nTry

  c = TCanvas('c','c',10,10,700,500)
  c.Divide(1,2)
  c.cd(1)
  h_chi2prob_initial_truth.Draw()
  c.cd(2)
  h_chi2prob_gbl_truth.Draw()
  ans = raw_input('kill...')
# 
  ''' 
Example #3
0
def main(args):
  '''
  Read initial fit and points from  test file
  Create trajectory from points,
  fit and write trajectory to MP-II binary file,
  get track parameter corrections and covariance matrix at points.
  
  Detector arrangement from text file
  '''  

  Chi2Sum = 0.
  NdfSum = 0
  LostSum = 0.

  np.random.seed(47117)

  if args.save:
    binaryFileName = "milleBinaryISN" + "_" + nametag
    binaryFile = open("%s.dat" % binaryFileName, "wb")

  inputFile = open(args.file, 'r')
  events = hpsevent.readHPSEvents(inputFile, args.nevents, args.ntracks)
  
  print 'Read %d events from file' % len(events)

  if len(events) > 0:
    Bz = events[0].Bz
  bfac = 0.0002998 * Bz # for Bz in Tesla, momentum in GeV and Radius in mm
  print Bz, bfac
  
  plots = hps_plots.plotter(nametag,'pdf',args.testrun,True,True, args.beamspot)
  plotsTop = hps_plots.plotter(nametag,'pdf',args.testrun,True,False, args.beamspot)
  plotsBot = hps_plots.plotter(nametag,'pdf',args.testrun,False,True,args.beamspot)
  
  #print " GblHpsTest $Rev: 234 $ ", nTry, nLayer
  nTry = 0
  start = time.clock()

  # loop over events
  for event in events:
    

    # loop over tracks in event
    for track in event.tracks:

      if ((nTry % 500) == 0 and nTry > 0) or args.debug: 
        print '\nProcessed %d events: now on event id %d and track id %d' % (nTry, event.id, track.id)

      # if there's no truth info -> skip the track
      # use the track parameters and check curvature
      if args.mc and track.curvature_truth() == 0.:
        print 'Track curvature is zero for this MC track, skip track ', track.id, ' in event ', event.id, ' (perigee pars truth: ', track.perParTruth,')'
        continue

      # check if it is top or bottom
      if args.debug:
        print 'track with strip0 origin ', track.strips[0].origin

      if args.notop and track.isTop():
          if args.debug:
            print 'not ok'
          continue
      if args.nobottom and not track.isTop():
          if args.debug:
            print 'not ok'
          continue
      if args.debug:
        print 'ok'
      
      # check if it has enough hits
      if len(track.strips) < args.minStrips:
        if args.debug:
          print 'not enough strip clusters'
        continue

      if args.minP != None and track.p(bfac) < args.minP:
          continue

    

      # create the trajectory
      traj = GblTrajectory(True)

      # save mapping between label and strip object 
      stripLabelMap = {}
      
      if args.debug: 
        print 'Track has %d strip clusters' % len(track.strips)

      # arc length
      s = 0.

      # point-to-point jacobian (from previous point)    
      jacPointToPoint = np.eye(5)

      #start trajectory at reference point (defining s=0)
      point = GblPoint(jacPointToPoint)
      refLabel = traj.addPoint(point)

      # multiple scattering covariance matrix (for curvilinear track parameters)
      msCov = np.zeros((5, 5))
      
      # store projections for later use
      proL2m_list = {} 

      # loop over strip clusters on the track

      for strip in track.strips:
        
        if args.debug:
          print '\nProcessing strip id %d, millepedeId %d on sensor %s origin (%f,%f,%f)' % (strip.id, strip.millepedeId,strip.deName,strip.origin[0],strip.origin[1],strip.origin[2])
        
        step = strip.pathLen3D - s

        if args.debug: print 'Path length step %f from %f to %f ' % (step, s, strip.pathLen3D)
        
        # measurement direction (in YZ plane: perpendicular/parallel to strip direction)
        mDir = np.array( [strip.u, strip.v] )
        
        if args.debug: print 'mDir:\n', mDir
        
        
        # track direction: in x directon
        sinLambda = strip.sinLambda
        cosLambda = math.sqrt(1.0 - sinLambda ** 2)
        sinPhi = strip.sinPhi
        cosPhi = math.sqrt(1.0 - sinPhi ** 2)
        
        if args.debug: print 'Track direction sinLambda=%f sinPhi=%f' % (sinLambda, sinPhi)

        # Track direction in the curvilinear frame (U,V,T)
        # U = Z x T / |Z x T|, V = T x U
        uvDir = np.array([[-sinPhi, cosPhi, 0.], \
                            [-sinLambda * cosPhi, -sinLambda * sinPhi, cosLambda]])
        

        if args.debug: print 'Track direction in curvilinear frame\n',uvDir
        
        # projection from  measurement to local (curvilinear uv) directions (duv/dm)
        proM2l = np.dot(uvDir, mDir.T)

        # projection from local (uv) to measurement directions (dm/duv)
        proL2m = np.linalg.inv(proM2l)
        proL2m_list[strip.id] = proL2m

        if args.debug: 
          print 'proM2l:\n', proM2l
          print 'proL2m:\n', proL2m

        # measurement/residual in the measurement system
        meas = np.array([strip.ures, 0.]) # only measurement in u-direction
        #meas[0] += deltaU[iLayer] # misalignment
        measErr = np.array([strip.ures_err, strip.ures_err])
        measPrec = 1.0 / measErr ** 2
        measPrec[1] = 0. # 1D measurement perpendicular to strip direction
        
        if args.debug: 
          print 'meas ', meas, ' measErr ', measErr, ' measPrec ', measPrec
      
        # cross-check track position and residuals
        if nTry < 10:
          
          uResIter = utils.getMeasurementResidualIterative(track.perPar,strip.origin,strip.u,strip.w,strip.meas,1.0e-8)
          #predIter = utils.getXPlanePositionIterative(track.perPar,strip.origin,strip.w,1.0e-8)
          #diffTrk = predIter - strip.origin
          #uPredIter = np.dot(strip.u , diffTrk.T)
          #uResIter = strip.meas - uPredIter
          if abs(uResIter - strip.ures) > 1.0e-6:
            print 'WARNING diff %.10f uResIter %.10f compared to %.10f' % (uResIter - strip.ures,uResIter,strip.ures)
            #print 'predIter ', predIter, ' origin ', strip.origin, ' diffTrk ',diffTrk,' u ', strip.u, ' diffTrk ',diffTrk.T
            sys.exit(1)
        

        # Find the Jacobian to be able to propagate the covariance matrix to this strip position
        jacPointToPoint = utils.gblSimpleJacobianLambdaPhi(step, cosLambda, abs(bfac))
        
        if args.debug: 
          print 'jacPointToPoint to extrapolate to this point:'
          print jacPointToPoint
        
        # propagate MS covariance matrix (in the curvilinear frame) to this strip position
        msCov = np.dot(jacPointToPoint, np.dot(msCov, jacPointToPoint.T))

        # Get the MS covariance for measurements in the measurement frame
        measMsCov = np.dot(proL2m, np.dot(msCov[3:, 3:], proL2m.T))
        
        # Plot the MS variance in the u-direction
        plots.h_measMsCov.Fill(float(strip.id+1),measMsCov[0,0])
        if track.isTop():
          plotsTop.h_measMsCov.Fill(float(strip.id+1),measMsCov[0,0])
        else:
          plotsBot.h_measMsCov.Fill(float(strip.id+1),measMsCov[0,0])
        
        if args.debug:
          print 'msCov at this point:'
          print msCov
          print 'measMsCov at this point:'
          print measMsCov
        
        # Option to blow up measurement error according to multiple scattering
        if args.useuncorrms:
          measPrec[0] = 1.0 / (measErr[0] ** 2 + measMsCov[0, 0])
          if args.debug:
            print 'Adding measMsCov ', measMsCov[0,0]
        
        # Create a GBL point
        point = GblPoint(jacPointToPoint)

        # Add measurement to the point
        point.addMeasurement([proL2m, meas, measPrec])

        # Add scatterer in curvilinear frame to the point
        # no direction in this frame
        scat = np.array([0., 0.])
        # Scattering angle in the curvilinear frame
        # Note the cosLambda to correct for the projection in the phi direction
        scatErr = np.array([ strip.scatAngle, strip.scatAngle / cosLambda]) 
        scatPrec = 1.0 / scatErr ** 2
        
        # add scatterer if not using the uncorrelated MS covariances
        if not args.useuncorrms:
          point.addScatterer([scat, scatPrec])
          if args.debug:
            print 'adding scatError to this point:'
            print scatErr
        
        # Update MS covariance matrix 
        msCov[1, 1] += scatErr[0] ** 2; msCov[2, 2] += scatErr[1] ** 2
        
                
        ##### 
        ## Calculate global derivatives for this point
        # track direction in tracking/global frame
        tDirGlobal = np.array( [ [cosPhi * cosLambda, sinPhi * cosLambda, sinLambda] ] )        
        # Cross-check that the input is consistent
        if( np.linalg.norm( tDirGlobal - strip.tDir) > 0.00001):
          print 'ERROR: tDirs are not consistent!'
          sys.exit(1)


        # Projection matrix from tracking frame to measurement frame
        # t_u = dot(u,i)*t_i + dot(u,j)*t_j + dot(u,k)*t_k
        # where t_u is component in new u direction and t_i is in the i direction
        prjTrkToMeas = np.array([strip.u,strip.v,strip.w])
        # rotate to measurement frame          
        tDirMeas = np.dot( prjTrkToMeas, tDirGlobal.T) 
        normalMeas = np.dot( prjTrkToMeas, np.array([strip.w]).T )
        # vector coplanar with measurement plane from origin to prediction
        tDiff = np.array( [strip.tPos]) - np.array( [strip.origin] )
        # rotate to measurement frame          
        tPosMeas = np.dot( prjTrkToMeas, tDiff.T) 

        if args.debug: 
          print 'tDirGlobal ', tDirGlobal
          print 'rotation matrix to meas frame\n', prjTrkToMeas
          print 'tPosGlobal ', np.array( [strip.tPos]) , ' (origin ', np.array( [strip.origin] ),')'
          print 'tDiff ', tDiff
          print 'tPosMeas ', tPosMeas
        plots.fillSensorPlots("pred_meas", strip.deName, tPosMeas)
        if track.isTop():
          plotsTop.fillSensorPlots("pred_meas", strip.deName, tPosMeas)
        else:
          plotsBot.fillSensorPlots("pred_meas", strip.deName, tPosMeas)

          
        # rotate track direction to measurement frame          
        # non-measured directions         
        vmeas = 0.
        wmeas = 0.

        # calculate and add derivatives to point
        glDers = utils.globalDers(strip.millepedeId,strip.meas,vmeas,wmeas,tDirMeas,tPosMeas,normalMeas)
        if args.debug:
          glDers.dump()
        ders = glDers.getDers(track.isTop())
        labGlobal = ders['labels']
        addDer = ders['ders']
        if args.debug:
          print 'global derivatives for strip ', strip.id, ' which has millepede id ', strip.millepedeId
          print labGlobal.shape
          for ider in range(labGlobal.shape[1]):
            print labGlobal[0][ider], '\t', addDer[0][ider]
        point.addGlobals(labGlobal, addDer)
        ##### 
        
        # add point to trajectory
        iLabel = traj.addPoint(point)

        #if nTry==0:
        #print 'uRes ', strip.id, ' uRes ', strip.ures, ' pred ', tPosMeas, ' s(3D) ', strip.pathLen3D
        #print 'uRes ', strip.id, ' uRes ', strip.ures, ' pred ', strip.tPos, ' s(3D) ', strip.pathLen3D
        
        # go to next point
        s += step

        # save strip and label map
        stripLabelMap[strip] = iLabel
      
      
      if args.debug: print 'Do the fit'
      Chi2, Ndf, Lost = traj.fit()

      # write to millepede
      if args.save:
        traj.milleOut(binaryFile)

      # sum up    
      Chi2Sum += Chi2
      NdfSum += Ndf
      LostSum += Lost

      if nTry == 0 or args.debug:
        print 'fit result: Chi2=%f Ndf=%d Lost=%d' % (Chi2, Ndf, Lost)
      

      # get corrections and covariance matrix at points; collect the result in one object
      result = hpsevent.GBLResults(track)

      #traj.dump()
      
      for i in range(1, traj.getNumPoints() + 1):      
        # label start at 1
        locPar, locCov = traj.getResults(-i)
        if nTry == 0:
          print " >Point ", i
          print " locPar ", locPar
          print " locCov ", locCov      
        result.addPoint(-i,locPar,locCov)
        locPar, locCov = traj.getResults(i)
        if nTry == -1:
          print " Point> ", i
          print " locPar ", locPar
          print " locCov ", locCov
        result.addPoint(i, locPar, locCov)
      
      if nTry == 0 or args.debug:
        result.printVertexCorr()
        result.printCorrection()
      
        

      # calculate the truth chi2 from initial fit
      # get the truth and fitted params with indexes same as cov matrix of initial fit (dca,phi0,curv,z0,slope)
      perParInitialVec = np.matrix([track.d0(), track.phi0(), track.curvature(), track.z0(), track.slope()])
      perParVecTruth = np.matrix([track.d0_truth(), track.phi0_truth(), track.curvature_truth(), track.z0_truth(), track.slope_truth()])
      perParInitialVecRes = perParInitialVec-perParVecTruth
      chi2_initial_truth = perParInitialVecRes * np.linalg.inv(track.perCov) * np.transpose(perParInitialVecRes)
      
      # calculate the truth chi2 from gbl fit at vertex
      clParGBLVtx = np.array(track.clPar) + np.array(result.locPar[1])
      clParTruth = np.array(track.clParTruth)
      clParGBLRes = clParGBLVtx - clParTruth
      chi2_gbl_truth = np.dot(clParGBLRes, np.dot(np.linalg.inv(result.locCov[1]), clParGBLRes))

      label = 1 #refLabel
      chi2_res = np.dot(result.locPar[label], np.dot(np.linalg.inv(result.locCov[label]), result.locPar[label]))
      if nTry == 0: 
        print " Chi2: ", event.id, chi2_res, chi2_gbl_truth, chi2_initial_truth
      




      # plots
      for iplot in range(3):
        plot = None
        if iplot==0:
          plot = plots
        elif iplot==1 and track.isTop():
          plot = plotsTop
        elif iplot==2 and not track.isTop():
          plot = plotsBot

        if plot is None:
          continue

        # reject some tracks
        #if abs(track.d0())<2.0:
        #  continue
        #if(track.q<0):
        #  continue
        
        plot.h_clPar_initial_xT.Fill(track.clPar[3])
        plot.h_clPar_initial_yT.Fill(track.clPar[4])
        plot.h_clPar_initial_qOverP.Fill(track.clPar[0])
        plot.h_clPar_initial_lambda.Fill(track.clPar[1])
        # transform phi to plot nicer
        if track.clPar[2]<math.pi:
          plot.h_clPar_initial_phi.Fill(track.clPar[2])
        else:
          plot.h_clPar_initial_phi.Fill(track.clPar[2]-math.pi*2)
        plot.h_clParGBL_res_qOverP.Fill(clParGBLRes[0])
        plot.h_clParGBL_res_lambda.Fill(clParGBLRes[1])
        plot.h_clParGBL_res_phi.Fill(clParGBLRes[2])
        plot.h_clParGBL_res_xT.Fill(clParGBLRes[3])
        plot.h_clParGBL_res_yT.Fill(clParGBLRes[4])

        plot.h_clParGBL_pull_qOverP.Fill(clParGBLRes[0]/math.sqrt(math.fabs(result.locCov[1][0,0])))
        plot.h_clParGBL_pull_lambda.Fill(clParGBLRes[1]/math.sqrt(math.fabs(result.locCov[1][1,1])))
        plot.h_clParGBL_pull_phi.Fill(clParGBLRes[2]/math.sqrt(math.fabs(result.locCov[1][2,2])))
        plot.h_clParGBL_pull_xT.Fill(clParGBLRes[3]/math.sqrt(math.fabs(result.locCov[1][3,3])))
        plot.h_clParGBL_pull_yT.Fill(clParGBLRes[4]/math.sqrt(math.fabs(result.locCov[1][4,4])))

        plot.h_perPar_res_initial_d0.Fill(perParInitialVecRes[0,0])
        plot.h_perPar_res_initial_phi0.Fill(perParInitialVecRes[0,1])
        plot.h_perPar_res_initial_kappa.Fill(perParInitialVecRes[0,2])
        plot.h_perPar_res_initial_z0.Fill(perParInitialVecRes[0,3])
        plot.h_perPar_res_initial_slope.Fill(perParInitialVecRes[0,4])
        plot.h_chi2_initial.Fill(track.chi2Initial)
        if track.ndfInitial != 0: 
          plot.h_chi2ndf_initial.Fill(track.chi2Initial/track.ndfInitial)
          plot.h_chi2prob_initial.Fill(utils.chi2Prob(track.chi2Initial,track.ndfInitial))
        else:
          plot.h_chi2ndf_initial.Fill(0.)
          plot.h_chi2prob_initial.Fill(-1.)
        plot.h_chi2_initial_truth.Fill(chi2_initial_truth)
        plot.h_chi2ndf_initial_truth.Fill(chi2_initial_truth/5.0)
        plot.h_chi2prob_initial_truth.Fill(utils.chi2Prob(chi2_initial_truth,5))
        plot.h_chi2_gbl_truth.Fill(chi2_gbl_truth)
        plot.h_chi2ndf_gbl_truth.Fill(chi2_gbl_truth/5.0)
        plot.h_chi2prob_gbl_truth.Fill(utils.chi2Prob(chi2_gbl_truth,5))
        plot.h_chi2.Fill(Chi2)
        plot.h_chi2ndf.Fill(Chi2/Ndf)
        plot.h_chi2prob.Fill(utils.chi2Prob(Chi2,Ndf))
        plot.h_p.Fill(track.p(bfac))
        plot.h_qOverP.Fill(track.qOverP(bfac))
        if args.mc:
          plot.h_qOverP_truth_res.Fill(track.qOverP(bfac) - track.q()/track.p_truth(bfac))   
          plot.h_p_truth.Fill(track.p_truth(bfac))
          plot.h_p_truth_res.Fill(track.p(bfac)-track.p_truth(bfac))
          plot.h_p_truth_res_vs_p.Fill(track.p_truth(bfac),track.p(bfac)-track.p_truth(bfac))


        plot.h_qOverP_corr.Fill(result.curvCorr())
        plot.h_qOverP_gbl.Fill(result.qOverP_gbl(bfac))
        plot.h_p_gbl.Fill(result.p_gbl(bfac))
        if args.mc:
          plot.h_qOverP_truth_res_gbl.Fill(result.qOverP_gbl(bfac) - result.track.qOverP_truth(bfac))
          plot.h_p_truth_res_gbl.Fill(result.p_gbl(bfac) - result.track.p_truth(bfac))
          plot.h_p_truth_res_gbl_vs_p.Fill(result.track.p_truth(bfac), result.p_gbl(bfac) - result.track.p_truth(bfac))


        vtx_idx = 1 # first point is at s=0 
        plot.h_vtx_xT_corr.Fill(result.xTCorr(vtx_idx))
        plot.h_vtx_yT_corr.Fill(result.yTCorr(vtx_idx))
        plot.h_d0_corr.Fill(result.d0Corr(vtx_idx))
        plot.h_z0_corr.Fill(result.z0Corr(vtx_idx))
        plot.h_d0_initial.Fill(track.d0())
        plot.h_z0_initial.Fill(track.z0())
        plot.h_d0_gbl.Fill(result.d0_gbl(vtx_idx))
        plot.h_z0_gbl.Fill(result.z0_gbl(vtx_idx))


        if args.debug:
          print 'curvCorr ', result.curvCorr(), ' xT_corr ', result.xTCorr(vtx_idx), ' yT_corr ', result.yTCorr(vtx_idx)
          print 'd0_corr ', result.d0Corr(vtx_idx), ' z0_corr ', result.z0Corr(vtx_idx)
          print 'd0_gbl ', result.d0_gbl(vtx_idx), ' (', result.track.d0(), ') z0_gbl ' , result.z0_gbl(vtx_idx), ' (', result.track.z0(), ')' 
          print 'locPar ', result.locPar[1]

        for label,corr in result.locPar.iteritems():
          if label>0:
            lbl = 2*(label-1) + 1
          else:
            lbl = -1*2*label
          plot.h_xT_corr.Fill(lbl, corr[result.idx_xT])
          plot.h_yT_corr.Fill(lbl, corr[result.idx_yT])

        for istrip in range(len(track.strips)):
          strip = track.strips[istrip]
            # find the label, if not found it's the vertex
          if strip in stripLabelMap:
            iLabel = stripLabelMap[strip]
          else:
            iLabel = 1

          #residuals
          plot.fillSensorPlots("res", strip.deName, strip.ures)
          plot.fillSensorPlots("res_truth", strip.deName, strip.uresTruth)
          #track direction corrections
          point = istrip + 2
          plot.fillSensorPlots("corr_lambda",strip.deName, result.locPar[point][result.idx_lambda])
          plot.fillSensorPlots("corrdiff_lambda",strip.deName, result.locPar[point][result.idx_lambda]-result.locPar[point-1][result.idx_lambda])
          plot.fillSensorPlots("corr_phi",strip.deName, result.locPar[point][result.idx_phi])
          plot.fillSensorPlots("corrdiff_phi",strip.deName, result.locPar[point][result.idx_phi]-result.locPar[point-1][result.idx_phi])
          # correction to xT,yT from GBL fit
          corr = np.matrix( [result.locPar[iLabel][3], result.locPar[iLabel][4] ] )
          # project to measurement direction
          corr_meas = np.matrix( proL2m_list[strip.id] ) * np.transpose( np.matrix( corr ) )
          ures_gbl = strip.ures - corr_meas[0,0] # note minus sign due to definition of residual
          plot.fillSensorPlots("res_gbl", strip.deName, ures_gbl)
          plot.fillSensorPlots("res_gbl_vs_vpred", strip.deName, [ures_gbl,tPosMeas[1]])
          if abs(strip.meas) > 20.:
            print 'really, this shouldnt happen? ', strip.meas
            sys.exit(1)
          #if abs(tPosMeas[1]) > 50.:
          #  print 'really2? ', tPosMeas
          #  #sys.exit(1)
          plot.fillSensorPlots("res_gbl_vs_u", strip.deName, [ures_gbl, strip.meas] )
          plot.fillSensorPlots("iso", strip.deName, strip.iso)

          # plot residuals of the seed vs the corrected seed
          if nTry < 999999:

            if args.debug: print '========= START DEBUG TRACK RESIDUALS ======== '

            # get the residual from the seed track perigee track parameters
            uResSeed = utils.getMeasurementResidualIterative(track.perPar,strip.origin,strip.u,strip.w,strip.meas,1.0e-8)

            # get the GBL corrections to the perigee track parameters at this point
            # NOTE: this is wrong!
            perParCorr = result.getPerParCorr(iLabel,bfac)
            if args.debug: print 'perPar     ', track.perPar
            if args.debug: print 'perParCorr ', perParCorr

            # get the residual from the *corrected* (see note above) seed track perigee track parameters
            uResSeedCorrWrong = utils.getMeasurementResidualIterative(perParCorr,strip.origin,strip.u,strip.w,strip.meas,1.0e-8)
            uResSeedCorrCmpWrong = abs(uResSeedCorrWrong) - abs(uResSeed)

            # plot the difference in residuals b/w the corrected and uncorrected track
            plot.fillSensorPlots("res_diff_wrong_gbl_seed", strip.deName, abs(uResSeedCorrWrong) - abs(uResSeed) )

            if args.debug: print 'WRONG diff ', uResSeedCorrCmpWrong, ' uResSeedCorrWrong', uResSeedCorrWrong, ' uResSeed ', uResSeed

            # This is the correct way of getting the corrected track parameters in perigee frame

            # Create a SimpleHelix object from the original seed track parameters
            # note that it uses slope instead of theta and difference ordering
            # [C,phi0,dca,slope,z0]
            helixSeed = simpleHelix.SimpleHelix([ track.perPar[0], track.perPar[2], track.perPar[3], math.tan(math.pi/2.0 - track.perPar[1]), track.perPar[4] ])

            if args.debug:
              print 'helixSeed '
              helixSeed.dump()

            # define reference points
            # global origin
            refPointAtOrg = [ 0., 0. ]
            # intersection of seed track with plane
            refPointAtPlane = [ strip.tPos[0],strip.tPos[1] ]

            if args.debug: print 'move helix to interception of seed track and plane which is at x,y ', refPointAtPlane, ' ( tPos ',strip.tPos,')'
            helixSeedParsAtPoint = helixSeed.moveToL3( refPointAtPlane )

            # create the helix at the new ref point
            helixSeedAtPoint = simpleHelix.SimpleHelix( helixSeedParsAtPoint, refPointAtPlane )

            if args.debug:
              print 'helixSeedAtPoint'
              helixSeedAtPoint.dump()
            
            # compare to the other propagation function
            if args.debug:
              helixSeedParsAtPointOther = helixSeed.moveTo( refPointAtPlane )
              helixSeedAtPointOther = simpleHelix.SimpleHelix( helixSeedParsAtPointOther, refPointAtPlane )
              print 'helixSeedAtPointOther'
              helixSeedAtPointOther.dump()
              print 'diff b/w propagations: ', np.array(helixSeedParsAtPoint) - np.array(helixSeedParsAtPointOther)
            
            # find the GBL corrections in perigee frame
            if args.debug: print 'get corrections in perFrame'
            perCorrections = result.getPerCorrections(iLabel, bfac)
            if args.debug: print 'perCorrection [C,theta,phi,d0,z0] ', perCorrections

            # get the corrections in the simpleHelix representation (slope instead of theta and different ordering)
            perCorrections = np.array( [ perCorrections[0], perCorrections[2], perCorrections[3], result.getSlopeCorrection(iLabel), perCorrections[4] ] )
            if args.debug: print 'perCorrection [C,phi,slope,d0,z0] ', perCorrections

            # cross-check the corrections in perigee frame with different formula
            perCorrectionsDiff = result.getPerCorrectionsOther(iLabel, bfac) - perCorrections
            #if args.debug: print 'perCorrectionOther ', perCorrectionsOther
            if args.debug: print 'Cross check perCorrections w/ different formula: ', perCorrectionsDiff
            if all( abs(i)>1e-5 for i in perCorrectionsDiff[:]):
                raise HpsGblException('Correction ', i, ' in perCorrections is different ', perCorrectionsDiff )
            
            # correct the perigee helix parameters at this point
            helixParsAtPoint = np.array( helixSeedAtPoint.getParameters() )            
            helixParsAtPointCorrected = helixParsAtPoint + perCorrections

            if args.debug:
              print 'helixParsAtPoint', helixParsAtPoint
              print 'perCorrections', perCorrections
              print 'helixParsAtPointCorrected', helixParsAtPointCorrected
            
            # create a SimpleHelix object from the corrected parameters
            helixCorrAtPoint = simpleHelix.SimpleHelix( helixParsAtPointCorrected, refPointAtPlane )

            if args.debug:
              print 'helixCorrAtPoint'
              helixCorrAtPoint.dump()

            # change reference point of the corrected helix to the original one
            #delta_refPointAtOrg = np.array( refPointAtOrg ) - np.array( refPointAtPlane )
            helixParsAtOrg = helixCorrAtPoint.moveToL3( refPointAtOrg )

            # create a SimpleHelix object from the corrected parameters at org
            helixCorr = simpleHelix.SimpleHelix( helixParsAtOrg, refPointAtOrg )

            if args.debug:
              print 'helixCorr'
              helixCorr.dump()

            # get the residual from the corrected track parameters at this point
            perParCorrSH = helixCorr.getParameters()
            if args.debug: print 'get residuals for parameters perParCorrSH ', perParCorrSH
            perParCorr = [ perParCorrSH[0], math.pi / 2.0 - math.atan( perParCorrSH[3] ), perParCorrSH[1], perParCorrSH[2], perParCorrSH[4] ]
            if args.debug: print ' and in [C,theta,phi,d0,z0] ', perParCorr
            uResSeedCorr = utils.getMeasurementResidualIterative(perParCorr,strip.origin,strip.u,strip.w,strip.meas,1.0e-8)
            uResSeedCorrCmp = abs(uResSeedCorr) - abs(uResSeed)

            # plot the difference in residuals b/w the corrected and uncorrected track
            plot.fillSensorPlots("res_diff_gbl_seed", strip.deName, abs(uResSeedCorr) - abs(uResSeed) )

            if args.debug: print 'CORR diff ', abs(uResSeedCorr) - abs(uResSeed), ' uResSeedCorr', uResSeedCorr, ' uResSeed ', uResSeed

            uResSeedCorrCmpVal = abs(uResSeedCorrCmp) - abs(uResSeedCorrCmpWrong)

            if args.debug: print 'CORR diff cmp ', uResSeedCorrCmpVal, ' uResSeedCorrCmp ', uResSeedCorrCmp, ' uResSeedCorrCmpWrong ', uResSeedCorrCmpWrong


            if args.debug: print '========= END DEBUG TRACK RESIDUALS ======== '


          # make plots for a given track only
          if nTry==0:
            plot.gr_ures.SetPoint(istrip,strip.pathLen3D,strip.ures)
            plot.gr_ures.SetPointError(istrip,0.,strip.ures_err)
            plot.gr_ures_truth.SetPoint(istrip,strip.pathLen3D,strip.uresTruth) 
            plot.gr_ures_simhit.SetPoint(istrip,strip.pathLen3D,strip.uresSimHit) 
            meass = np.array([strip.ures, 0.])
            # find corrections to xT and yT
            plot.gr_corr_ures.SetPoint(istrip, strip.pathLen3D, corr_meas[0,0]) #u-direction
            ures_corr =  meass - corr_meas.T
            plot.gr_ures_corr.SetPoint(istrip, strip.pathLen3D, ures_corr[0,0]) #u-direction
      
      nTry += 1

      
  #
  end = time.clock()
  print " Processed %d tracks " % nTry
  print " Time [s] ", end - start
  if nTry > 0:
    print " Chi2Sum/NdfSum ", Chi2Sum / NdfSum
    print " LostSum/nTry ", LostSum / nTry
  print " Make plots "
  if nTry > 0 and not args.noshow:
    plots.show(args.save,args.nopause)
    plotsTop.show(args.save,args.nopause)
    plotsBot.show(args.save,args.nopause)
    if args.save:
      hps_plots.saveHistosToFile(gDirectory,'gbltst-hps-plots-%s.root' % nametag)
Example #4
0
def exampleHpsTest():
  '''
  Create points on initial trajectory, create trajectory from points,
  fit and write trajectory to MP-II binary file,
  get track parameter corrections and covariance matrix at points.
  
  Detector arrangement according to HPS test setup 2012, B=0.
  '''  
  def gblSimpleJacobian(ds, cosl, bfac):
    '''
    Simple jacobian: quadratic in arc length difference (parabola instead of circle),
    assuming constant magnetic field in Z direction.
    
    @param ds: arc length difference
    @type ds: float
    @param cosl: cos(lambda)
    @type cosl: float
    @param bfac: Bz*c
    @type bfac: float
    @return: jacobian to move by 'ds' on trajectory
    @rtype: matrix(float)
    '''
    jac = np.eye(5)
    jac[1, 0] = -bfac * ds * cosl
    jac[3, 0] = -0.5 * bfac * ds * ds * cosl
    jac[3, 1] = ds
    jac[4, 2] = ds  
    return jac
#
  np.random.seed(47117)

  nTry = 1000 #: number of tries
  nLayer = 10   #: number of detector layers
  # positions (perpendicular to detector plane)
  positions = [88., 95., 188., 195., 288., 295., 488., 495., 688., 695.]
  # stereo angles
  angles = [0., 0.005, 0., 0.005, 0., 0.005, 0., 0.01, 0., 0.01 ]
  # misalignment in measurement direction
  deltaU = [0., 0., 0.0, 0.00, 0., 0., 0., 0., 0., 0.]
  print " GblHpsTest $Rev: 234 $ ", nTry, nLayer
  start = time.clock()
# track direction: in x direction
  sinLambda = 0.
  cosLambda = math.sqrt(1.0 - sinLambda ** 2)
  sinPhi = 0.
  cosPhi = math.sqrt(1.0 - sinPhi ** 2)
#  tDir = np.array([cosLambda * cosPhi, cosLambda * sinPhi, sinLambda])
# U = Z x T / |Z x T|, V = T x U
  uvDir = np.array([[-sinPhi, cosPhi, 0.], \
                  [-sinLambda * cosPhi, -sinLambda * sinPhi, cosLambda]])
# measurement resolution
  measErr = np.array([ 0.006, 0.006]) # 6 mu
  measPrec = 1.0 / measErr ** 2
  measPrec[1] = 0.0 # only 1D measurement (perpendicular to strip direction) 
# scattering error
  scatErr = np.array([ 0.000070, 0.000070]) # 70 micro-rad
  scatPrec = 1.0 / scatErr ** 2
# RMS of track parameters
  clErr = np.array([0.001, 0.05, 0.05, 1., 1.])
  clSeed = np.eye(5)
  for i in range(5):
    clSeed[i, i] = 1.0 / clErr[i] ** 2
#
  bfac = 0.#2998 # Bz*c for Bz=1  (units:MeV, T, mm)
#
  Chi2Sum = 0.
  NdfSum = 0
  LostSum = 0.
#
  binaryFile = open("milleBinaryISN.dat", "wb")
#
  for iTry in range(nTry):
# generate (CurviLinear) track parameters 
    clNorm = np.random.normal(0., 1., 5)  
    clPar = clErr * clNorm
# arclength
    s = 0.
    sPoint = []
# point-to-point jacobian (from previous point)    
    jacPointToPoint = np.eye(5)
# additional (local or global) derivatives    
    addDer = np.array([[1.0], [0.0]])
    labGlobal = np.array([[4711], [4711]])
# create trajectory
    traj = GblTrajectory(bfac != 0.)
    
    for iLayer in range(nLayer):
#     step
      step = positions[iLayer] / cosLambda - s
      prop = np.array([[1., step], [0., 1.]])
      #print " layer ", iLayer, step, varMs
#     measurement directions (in YZ plane: perpendicular/parallel to strip direction)  
      sinStereo = angles[iLayer] 
      cosStereo = math.sqrt(1.0 - sinStereo ** 2)    
      mDir = np.array([[0., sinStereo, cosStereo], [0., cosStereo, -sinStereo]])
# projection measurement to local (curvilinear uv) directions (duv/dm)
      proM2l = np.dot(uvDir, mDir.T)
# projection local (uv) to measurement directions (dm/duv)
      proL2m = np.linalg.inv(proM2l)
# measurement - prediction in measurement system with error      
      measNorm = np.random.normal(0., 1., 2)  
      meas = np.dot(proL2m, clPar[3:5]) + measErr * measNorm
      meas[0] += deltaU[iLayer] # misalignment
# point with measurement
      #measPrec[0] = 1.0 / (varMs[0][0] + measErr[0] * measErr[0])
      point = GblPoint(jacPointToPoint)
      point.addMeasurement([proL2m, meas, measPrec])
# point with scatterer
      scat = np.array([0., 0.])
      point.addScatterer([scat, scatPrec])
# scatter a little    
      scatNorm = np.random.normal(0., 1., 2)  
      clPar[1:3] = clPar[1:3] + scatErr * scatNorm      
# additional global parameters?
      addDer = np.array([[1.0], [0.0]])
      labGlobal = np.array([[11101 + iLayer], [0]])
      point.addGlobals(labGlobal, addDer)
# add point to trajectory      
      iLabel = traj.addPoint(point)
      sPoint.append(s)
# propagate to scatterer
      jacPointToPoint = gblSimpleJacobian(step, cosLambda, bfac)
      clPar = np.dot(jacPointToPoint, clPar)
      s += step
 
# add external seed    
#    traj.addExternalSeed(1, clSeed)
# dump trajectory
#    traj.dump()
  
# fit trajectory
    Chi2, Ndf, Lost = traj.fit()
    #print " Record, Chi2, Ndf, Lost", iTry, Chi2, Ndf, Lost
# write to MP binary file    
    traj.milleOut(binaryFile)
# sum up    
    Chi2Sum += Chi2
    NdfSum += Ndf
    LostSum += Lost
# get corrections and covariance matrix at points 
    if (iTry == 0):
      for i in range(1, 1):      
        locPar, locCov = traj.getResults(i)
        print " Point< ", i
        print " locPar ", locPar
        print " locCov ", locCov      
        traj.getResults(-i)
        print " Point> ", i
        print " locPar ", locPar
        print " locCov ", locCov  
#
  end = time.clock()
  print " Time [s] ", end - start
  print " Chi2Sum/NdfSum ", Chi2Sum / NdfSum
  print " LostSum/nTry ", LostSum / nTry