View Javadoc

1   /*
2    * ZSegmentFitter.java
3    *
4    * Created on July 29, 2007, 9:43 PM
5    *
6    */
7   
8   package org.lcsim.fit.zsegment;
9   
10  import hep.physics.matrix.BasicMatrix;
11  import hep.physics.matrix.Matrix;
12  import hep.physics.matrix.MatrixOp;
13  import hep.physics.matrix.MutableMatrix;
14  import hep.physics.matrix.SymmetricMatrix;
15  
16  import java.util.ArrayList;
17  import java.util.List;
18  
19  /**
20   * Find the allowed region in z0 - tan(lambda) track parameter space for a track that
21   * passes through a set of detectors segmented in the z coordinate.  The z coordinates
22   * for each measurement are treated as being bounded but unmeasured.  The allowed region
23   * is a polygon in z0-tan(lambda) space with 3 or more vertices.  The centroid and
24   * covariance matrix for the allowed region are also calculated.
25   * @author Richard Partridge
26   * @version 1.0
27   */
28  public class ZSegmentFitter {
29      
30      private double[] _s;
31      private double[] _zmin;
32      private double[] _zmax;
33      private List<double[]> _polygon;
34      private double _area;
35      private double[] _centroid = new double[2];
36      private SymmetricMatrix _covariance;
37      private double _eps = 1e-6;
38      
39      /**
40       * Create a new instance of ZSegmentFitter
41       */
42      public ZSegmentFitter() {
43          
44      }
45      
46      /**
47       * Find the allowed region in z0-tan(lambda) space and determine the centroid and
48       * covariance matrix for this region.
49       * @param s Array specifying the arc length in the x-y plane for each hit
50       * @param zmin Array specifying the minimum z coordinate for each hit
51       * @param zmax Array specifying the maximum z coordinate for each hit
52       * @return True if the specified hits are consistent with a straight line in the s-z plane
53       */
54      public boolean fit(double[] s, double[] zmin, double[] zmax) {
55  //  Save the fit input values and initialize the fit results now in case we return without finding a successful fit
56          _s = s;
57          _zmin = zmin;
58          _zmax = zmax;
59          _polygon = new ArrayList<double[]>();
60          _centroid[0] = 0.;
61          _centroid[1] = 0.;
62          _area = 0.;
63          _covariance = null;
64  //  Check that the input values are sensible
65          if (_s.length!=_zmin.length) return false;
66          if (_s.length!=_zmax.length) return false;
67          if (_s.length<2) return false;
68          
69  //  Each z segment specfies an allowed band in the z0-tan(lambda) plane given by
70  //  the constraint zmin < z0 + s*tan(lambda) < zmax.  The edges of each band are
71  //  bounded by the lines zmin = z0 + s*tan(lambda) and zmax = z0 + s*tan(lambda).
72  //  For each pair of measurements, the intersection of the corresponding bands is
73  //  a parallelogram in z0-tan(lambda) space.  The four vertices of the parallelogram
74  //  are found by intersecting the lines specifying the band edges given above.
75  //  The allowed region in z0-tan(lambda) space will be a convex polygon whose vertices
76  //  are a subset of all parallelogram vertices found in considering all layer pairs.
77  //  Each parallelogram vertex specifies a specific point in z0-tan(lambda) space
78  //  that may or may not be consistent with the full set of z segments.  Those
79  //  parallelogram vertices that are consistent with all z segments are retained
80  //  as polygon vertices that specify the allowed region in z-tan(lambda) space.
81          for (int i=0; i<_s.length-1; i++) {
82              for (int j=i+1; j<_s.length; j++) {
83                  IntersectLines(_s[i],_zmin[i],_s[j],_zmin[j]);
84                  IntersectLines(_s[i],_zmin[i],_s[j],_zmax[j]);
85                  IntersectLines(_s[i],_zmax[i],_s[j],_zmin[j]);
86                  IntersectLines(_s[i],_zmax[i],_s[j],_zmax[j]);
87              }
88          }
89          
90  //  Check that we have at least 3 polygon vertices - fewer vertices indicates that the
91  //  specified z segments are not consistent with a straight line track in s-z space.
92          int nv = _polygon.size();
93          if (nv<3) return false;
94          
95  //  Order the vertices so that adjacent vertices describe a line segment in the
96  //  polygon
97          OrderVertices();
98          
99  //  Find the area and centroid of the polygon (see, for example, http://local.wasp.uwa.edu.au/~pbourke/geometry/polyarea/)
100         for (int i=0; i<nv; i++) {
101             int j = (i+1) % nv;
102             double[] p0 = _polygon.get(i);
103             double[] p1 = _polygon.get(j);
104             double darea = 0.5*(p0[0]*p1[1] - p1[0]*p0[1]);
105             _area += darea;
106             _centroid[0] += (p0[0]+p1[0]) * darea;
107             _centroid[1] += (p0[1]+p1[1]) * darea;
108         }
109         _centroid[0] /= 3*_area;
110         _centroid[1] /= 3*_area;
111         _area = Math.abs(_area);
112         
113 //  Calculate the covariance matrix for this polygon
114         _covariance = PolygonCovariance();
115         
116         return true;
117     }
118     
119     private void IntersectLines(double s1, double o1, double s2, double o2) {
120 //  Find the intersection of the lines z0 = 01 - s1*tan(lambda) and z0 = o2 - s2*tan(lambda)
121         double[] cross = new double[2];
122         if (s1==s2) return;
123         cross[0] = (o1*s2 - o2*s1) / (s2 - s1);
124         cross[1] = (o2 - o1) / (s2 - s1);
125         
126 //  See if this intersection is consistent with all z segments
127         for (int i=0; i<_s.length; i++) {
128             double zpred = cross[0] + _s[i]*cross[1];
129             if (zpred<_zmin[i]-_eps || zpred>_zmax[i]+_eps ) return;
130         }
131         
132 //  See if this intersection duplicates one we have previously found
133         for (double[] old_cross : _polygon) {
134             if (Math.pow(cross[0]-old_cross[0],2)+Math.pow(cross[1]-old_cross[1],2)<Math.pow(_eps,2)) return;
135         }
136         
137 //  Add this intersection to our polygon
138         _polygon.add(cross);
139     }
140     
141     private void OrderVertices() {
142 //  Take as an origin a point within the polygon and order the polygon vertices according to their azimuthal angle
143         double[] pcent = PseudoCentroid();
144         int nv = _polygon.size();
145         for (int i=0; i<nv-1; i++) {
146             for (int j=i+1; j<nv; j++) {
147                 //  phi1 calcuation must stay inside loop because of possible re-ordering
148                 double phi1 = Math.atan2(_polygon.get(i)[1]-pcent[1],_polygon.get(i)[0]-pcent[0]);
149                 double phi2 = Math.atan2(_polygon.get(j)[1]-pcent[1],_polygon.get(j)[0]-pcent[0]);
150                 if (phi1>phi2) {
151                     double[] temp = _polygon.get(j);
152                     _polygon.set(j,_polygon.get(i));
153                     _polygon.set(i,temp);
154                 }
155             }
156         }
157     }
158     
159     private double[] PseudoCentroid() {
160         // Find a point within the convex polygon by averaging the coordinates of all vertices
161         double[] pcent = {0.,0.};
162         int nv = _polygon.size();
163         for (double[] point : _polygon) {
164             pcent[0] += point[0]/ nv;
165             pcent[1] += point[1]/ nv;
166         }
167         return pcent;
168     }
169     
170     private SymmetricMatrix PolygonCovariance() {
171 //  Calculate the covariance matrix for a convex polygon assuming all points in the polygon are equally likely.
172 //  The algorithm used here is to break the polygon into triangles, each of which contains the polygon centroid,
173 //  and calculate the contribution to the covariance matrix from each such triangle.
174         
175         int nv = _polygon.size();
176         double cxx = 0.;
177         double cxy = 0.;
178         double cyy = 0.;
179         for (int i=0; i<nv; i++) {
180             int j = (i+1) % nv;
181 //  Find the triangle vertices in a coordinate system whose origin is at the polygon centroid.  Store the
182 //  two vertices as two columns in a 2x2 matrix (we ignore the 3rd triangle vertex located at the centroid)
183             BasicMatrix vertices = new BasicMatrix(2,2);
184             vertices.setElement(0,0,_polygon.get(i)[0]-_centroid[0]);
185             vertices.setElement(1,0,_polygon.get(i)[1]-_centroid[1]);
186             vertices.setElement(0,1,_polygon.get(j)[0]-_centroid[0]);
187             vertices.setElement(1,1,_polygon.get(j)[1]-_centroid[1]);
188             
189 //  Rotate these vertices to a new x',y' coordinate system where vertex 2 is on the x' axis
190             double phi = Math.atan2(vertices.e(1,1),vertices.e(0,1));
191             BasicMatrix rotmat = new BasicMatrix(2,2);
192             rotmat.setElement(0,0,Math.cos(phi));
193             rotmat.setElement(0,1,Math.sin(phi));
194             rotmat.setElement(1,0,-rotmat.e(0,1));
195             rotmat.setElement(1,1,rotmat.e(0,0));
196             Matrix rotvert = MatrixOp.mult(rotmat,vertices);
197             
198 //  Find the contributions to the covariance matrix for the triangle in the x',y' coordinate system.  If
199 //  the apex of the triangle is at (x'1,y'1), and the base of the triangle is at (0,0) and (x'2,0), then:
200 //      A = 1/2 y'1 * x'2
201 //      Ix'x' = integral x'*x'*dA = A * (x'1**2 + x'1*x'2 + x'2**2) / 6
202 //      Ix'y' = integral x'*y'*dA = A * y'1 * (2*x'1 + x'2) / 12
203 //      Iy'y' = integral y'*y'*dA = A * y'1**2 / 6
204             double darea = Math.abs(0.5*rotvert.e(1,0)*rotvert.e(0,1));
205             SymmetricMatrix cov_loc = new SymmetricMatrix(2);
206             cov_loc.setElement(0,0,darea*(Math.pow(rotvert.e(0,0),2)+rotvert.e(0,0)*rotvert.e(0,1)+Math.pow(rotvert.e(0,1),2))/6.);
207             cov_loc.setElement(1,0,darea*rotvert.e(1,0)*(2.*rotvert.e(0,0)+rotvert.e(0,1))/12);
208             cov_loc.setElement(1,1,darea*Math.pow(rotvert.e(1,0),2)/6);
209             
210 //  Rotate the 2nd rank tensor back to the local coordinate system: v vT = RT * v' v'T R where R is the rotation matrix,
211 //  v / v' are the (x,y) / (x',y') column vectors, and T indicates the transpose operator.  Note that v' = R v and R RT = RT R = 1.
212             MutableMatrix rotbar = new BasicMatrix(2,2);
213             MatrixOp.transposed(rotmat,rotbar);
214             Matrix cov_glb = MatrixOp.mult(rotbar,MatrixOp.mult(cov_loc,rotmat));
215             
216 //  Add the covariance contribution for this triangle to the polygon sum (note - we need a matrix add operation in freehep !!)
217             cxx += cov_glb.e(0,0);
218             cxy += cov_glb.e(1,0);
219             cyy += cov_glb.e(1,1);
220         }
221         
222 //  Create the covariance matrix by dividing out the polygon area: cov_xx = integral x*x*dA / integral dA
223         SymmetricMatrix cov = new SymmetricMatrix(2);
224         cov.setElement(0,0,cxx/_area);
225         cov.setElement(1,0,cxy/_area);
226         cov.setElement(1,1,cyy/_area);
227         
228         return cov;
229     }
230     
231     /**
232      * Return the resutls of the fit as a ZSegmentFit object
233      * @return ZSegmentFitter fit result
234      */
235     public ZSegmentFit getFit() {
236         return new ZSegmentFit(_polygon, _centroid, _covariance);
237     }
238 }