5 #include "CircleHough.h"
11 using namespace Eigen;
14 ZHough_Cylindrical::ZHough_Cylindrical(
unsigned int z0_nbin,
unsigned int theta_nbin, ZResolution& min_resolution, ZResolution& max_resolution, ZRange& range,
double sxy,
double sz) : ZHough(z0_nbin, theta_nbin, min_resolution, max_resolution, range, sxy, sz), nlayers(6)
26 void ZHough_Cylindrical::findTracks(
unsigned int zoomlevel,
float xydiffcut,
unsigned int max_hits,
unsigned int tracks_per_hit,
double chi2_cut,
float max_kappa_cut, vector<SimpleTrack3D>& tracks, vector<SimpleHit3D>&
hits,
float min_k,
float max_k,
float min_phi,
float max_phi,
float min_d,
float max_d,
float min_z0,
float max_z0,
float min_theta,
float max_theta, vector<float>& params)
30 cout<<
"findTracks"<<endl;
31 cout<<min_k<<
" "<<max_k<<
" "<<min_phi<<
" "<<max_phi<<
" "<<min_d<<
" "<<max_d<<
" "<<min_z0<<
" "<<max_z0<<
" "<<min_theta<<
" "<<max_theta<<endl;
34 vector<SimpleHit3D> comb_hits = (*(hits_vec[zoomlevel+1]));
35 if(using_vertex==
false)
37 findTracksCombo_noVertex(comb_hits, zoomlevel, xydiffcut, max_hits, tracks_per_hit, chi2_cut, max_kappa_cut, tracks, hits, min_k, max_k, min_phi, max_phi, min_d, max_d, min_z0, max_z0, min_theta, max_theta, params);
41 findTracksCombo_withVertex(comb_hits, zoomlevel, xydiffcut, max_hits, tracks_per_hit, chi2_cut, max_kappa_cut, tracks, hits, min_k, max_k, min_phi, max_phi, min_d, max_d, min_z0, max_z0, min_theta, max_theta, params);
47 void ZHough_Cylindrical::findTracksCombo_noVertex(vector<SimpleHit3D>& comb_hits,
unsigned int zoomlevel,
float xydiffcut,
unsigned int max_hits,
unsigned int tracks_per_hit,
double chi2_cut,
float max_kappa_cut, vector<SimpleTrack3D>& tracks, vector<SimpleHit3D>&
hits,
float min_k,
float max_k,
float min_phi,
float max_phi,
float min_d,
float max_d,
float min_z0,
float max_z0,
float min_theta,
float max_theta, vector<float>& params)
49 unsigned int req_hits = (
unsigned int)(params[2]);
51 vector<unsigned int> index_holder;
52 index_holder.assign(comb_hits.size(), 0);
53 for(
unsigned int i=0;i<comb_hits.size();++i)
55 index_holder[i]=comb_hits[i].index;
56 comb_hits[i].index = i;
60 vector<double> chi2_hits;
62 vector<bool> use_here;
63 use_here.assign(comb_hits.size(),
true);
65 temp_track.
hits.resize(3, comb_hits[0]);
68 for(
unsigned int i1=0;i1<comb_hits.size();++i1)
70 if(use_here[i1]==
false){
continue;}
71 if(used_vec[index_holder[i1]] >= tracks_per_hit){
continue;}
72 temp_track.
hits[0] = comb_hits[i1];
73 for(
unsigned int i2=(i1+1);i2<comb_hits.size();++i2)
75 if(use_here[i2]==
false){
continue;}
76 if(used_vec[index_holder[i1]] >= tracks_per_hit){
continue;}
77 if(used_vec[index_holder[i2]] >= tracks_per_hit){
continue;}
78 if(comb_hits[i2].
layer == comb_hits[i1].
layer){
continue;}
79 phidiff = temp_track.
hits[0].get_phi() - comb_hits[i2].get_phi();
80 if(phidiff < -
M_PI){phidiff += 2.*
M_PI;}
81 if(phidiff >
M_PI){phidiff -= 2.*
M_PI;}
82 if(fabs(phidiff) >
M_PI/3.){
continue;}
83 temp_track.
hits[1] = comb_hits[i2];
84 float xdiff2 = (temp_track.
hits[0].get_x() - temp_track.
hits[1].get_x());
86 float ydiff2 = (temp_track.
hits[0].get_y() - temp_track.
hits[1].get_y());
88 float xydiff2 = xdiff2 + ydiff2;
89 float zdiff2 = (temp_track.
hits[0].get_z() - temp_track.
hits[1].get_z());
91 for(
unsigned int i3=(i2+1);i3<comb_hits.size();++i3)
93 if(use_here[i3]==
false){
continue;}
94 if(used_vec[index_holder[i1]] >= tracks_per_hit){
continue;}
95 if(used_vec[index_holder[i2]] >= tracks_per_hit){
continue;}
96 if(used_vec[index_holder[i3]] >= tracks_per_hit){
continue;}
97 if((comb_hits[i3].layer == comb_hits[i2].layer) || (comb_hits[i3].layer == comb_hits[i1].layer)){
continue;}
98 phidiff = temp_track.
hits[1].get_phi() - comb_hits[i3].get_phi();
99 if(phidiff < -
M_PI){phidiff += 2.*
M_PI;}
100 if(phidiff >
M_PI){phidiff -= 2.*
M_PI;}
101 if(fabs(phidiff) >
M_PI/3.){
continue;}
102 temp_track.
hits[2] = comb_hits[i3];
103 float xdiff2_2 = (temp_track.
hits[1].get_x() - temp_track.
hits[2].get_x());
105 float ydiff2_2 = (temp_track.
hits[1].get_y() - temp_track.
hits[2].get_y());
107 float xydiff2_2 = xdiff2_2 + ydiff2_2;
108 float zdiff2_2 = (temp_track.
hits[1].get_z() - temp_track.
hits[2].get_z());
111 if(fabs(xydiff2*zdiff2_2/(zdiff2*xydiff2_2) - 1.) > 0.2){
continue;}
113 temp_track.
hits.resize(3, comb_hits[0]);
114 chi2 =
fitTrack(temp_track, chi2_hits);
115 if((chi2 < chi2_cut)&&(temp_track.
kappa < max_kappa_cut))
118 vector<SimpleTrack3D> ttracks;
120 for(
unsigned int j=0;j<temp_track.
hits.size();++j)
122 ttrack.
hits[j].index = index_holder[ttrack.
hits[j].index];
123 ttrack.
hits[j].index = index_mapping[ttrack.
hits[j].index];
125 ttracks.push_back(ttrack);
126 unsigned int start_size = tracks.size();
127 chough->addHits((
unsigned int)(params[4]), ttracks, tracks, params, tracks_per_hit, params[5]);
128 if(tracks.size() > start_size)
130 if(tracks.back().hits.size() >= req_hits)
132 unsigned int count = 0;
133 for(
unsigned int j=0;j<temp_track.
hits.size();++j)
135 use_here[temp_track.
hits[j].index] =
false;
137 for(
unsigned int j=0;j<temp_track.
hits.size();++j)
139 used_vec[index_holder[temp_track.
hits[j].index]]++;
142 for(
unsigned int j=temp_track.
hits.size();j<tracks.back().hits.size();++j)
144 for(
unsigned int i=0;i<(*(hits_vec[zoomlevel+1])).size();++i)
146 if(tracks.back().hits[j].index == index_mapping[(*(hits_vec[zoomlevel+1]))[i].index])
148 used_vec[(*(hits_vec[zoomlevel+1]))[i].index]++;
160 for(
unsigned int i=0;i<comb_hits.size();++i)
162 comb_hits[i].index = index_holder[i];
167 void ZHough_Cylindrical::findTracksCombo_withVertex(vector<SimpleHit3D>& comb_hits,
unsigned int zoomlevel,
float xydiffcut,
unsigned int max_hits,
unsigned int tracks_per_hit,
double chi2_cut,
float max_kappa_cut, vector<SimpleTrack3D>& tracks, vector<SimpleHit3D>&
hits,
float min_k,
float max_k,
float min_phi,
float max_phi,
float min_d,
float max_d,
float min_z0,
float max_z0,
float min_theta,
float max_theta, vector<float>& params)
169 unsigned int req_hits = (
unsigned int)(params[2]);
171 vector<unsigned int> index_holder;
172 index_holder.assign(comb_hits.size(), 0);
173 for(
unsigned int i=0;i<comb_hits.size();++i)
175 index_holder[i]=comb_hits[i].index;
176 comb_hits[i].index = i;
180 vector<double> chi2_hits;
182 vector<bool> use_here;
183 use_here.assign(comb_hits.size(),
true);
185 temp_track.
hits.resize(2, comb_hits[0]);
188 for(
unsigned int i1=0;i1<comb_hits.size();++i1)
190 if(use_here[i1]==
false){
continue;}
191 if(used_vec[index_holder[i1]] >= tracks_per_hit){
continue;}
192 temp_track.
hits[0] = comb_hits[i1];
194 float xdiff2_v = (temp_track.
hits[0].get_x());
196 float ydiff2_v = (temp_track.
hits[0].get_y());
198 float xydiff2_v = xdiff2_v + ydiff2_v;
199 float zdiff2_v = (temp_track.
hits[0].get_z());
202 for(
unsigned int i2=(i1+1);i2<comb_hits.size();++i2)
204 if(use_here[i1]==
false){
continue;}
205 if(use_here[i2]==
false){
continue;}
206 if(comb_hits[i2].
layer == comb_hits[i1].
layer){
continue;}
207 if(used_vec[index_holder[i1]] >= tracks_per_hit){
continue;}
208 if(used_vec[index_holder[i2]] >= tracks_per_hit){
continue;}
209 phidiff = temp_track.
hits[0].get_phi() - comb_hits[i2].get_phi();
210 if(phidiff < -
M_PI){phidiff += 2.*
M_PI;}
211 if(phidiff >
M_PI){phidiff -= 2.*
M_PI;}
212 if(fabs(phidiff) >
M_PI/3.){
continue;}
213 temp_track.
hits[1] = comb_hits[i2];
214 float xdiff2 = (temp_track.
hits[0].get_x() - temp_track.
hits[1].get_x());
216 float ydiff2 = (temp_track.
hits[0].get_y() - temp_track.
hits[1].get_y());
218 float xydiff2 = xdiff2 + ydiff2;
219 float zdiff2 = (temp_track.
hits[0].get_z() - temp_track.
hits[1].get_z());
221 if(fabs(xydiff2*zdiff2_v/(zdiff2*xydiff2_v) - 1.) > 0.2){
continue;}
223 temp_track.
hits.resize(2, comb_hits[0]);
224 chi2 =
fitTrack(temp_track, chi2_hits);
225 if((chi2 < chi2_cut)&&(temp_track.
kappa < max_kappa_cut))
228 vector<SimpleTrack3D> ttracks;
230 for(
unsigned int j=0;j<temp_track.
hits.size();++j)
232 ttrack.
hits[j].index = index_holder[ttrack.
hits[j].index];
233 ttrack.
hits[j].index = index_mapping[ttrack.
hits[j].index];
235 ttracks.push_back(ttrack);
236 unsigned int start_size = tracks.size();
237 chough->addHits((
unsigned int)(params[4]), ttracks, tracks, params, tracks_per_hit, params[5]);
238 if(tracks.size() > start_size)
240 if(tracks.back().hits.size() >= req_hits)
242 unsigned int count = 0;
243 for(
unsigned int j=0;j<temp_track.
hits.size();++j)
245 use_here[temp_track.
hits[j].index] =
false;
247 for(
unsigned int j=0;j<temp_track.
hits.size();++j)
249 used_vec[index_holder[temp_track.
hits[j].index]]++;
252 for(
unsigned int j=temp_track.
hits.size();j<tracks.back().hits.size();++j)
254 for(
unsigned int i=0;i<(*(hits_vec[zoomlevel+1])).size();++i)
256 if(tracks.back().hits[j].index == index_mapping[(*(hits_vec[zoomlevel+1]))[i].index])
258 used_vec[(*(hits_vec[zoomlevel+1]))[i].index]++;
274 if(using_vertex ==
true)
280 chi2_hit.resize(track.
hits.size(), 0.);
282 MatrixXd
y = MatrixXd::Zero(track.
hits.size(), 1);
283 for(
unsigned int i=0;i<track.
hits.size();i++)
285 y(i, 0) = ( pow(track.
hits[i].get_x(),2) + pow(track.
hits[i].get_y(),2) );
290 MatrixXd
X = MatrixXd::Zero(track.
hits.size(), 3);
291 for(
unsigned int i=0;i<track.
hits.size();i++)
293 X(i, 0) = track.
hits[i].get_x();
294 X(i, 1) = track.
hits[i].get_y();
296 if((using_vertex==
true ) && (i == (track.
hits.size() - 1)))
310 MatrixXd Xt = X.transpose();
312 MatrixXd prod = Xt*
X;
313 MatrixXd inv = prod.fullPivLu().inverse();
315 MatrixXd beta = inv*Xt*
y;
317 double cx = beta(0,0)*0.5;
318 double cy = beta(1,0)*0.5;
319 double r = sqrt(cx*cx + cy*cy - beta(2,0));
321 double phi = atan2(cy, cx);
322 double d = sqrt(cx*cx + cy*cy) -
r;
325 MatrixXd diff = y - (X*beta);
326 MatrixXd chi2 = (diff.transpose())*diff;
327 for(
unsigned int i=0;i<track.
hits.size();i++)
329 chi2_hit[i] += diff(i,0)*diff(i,0);
332 double dx = d*cos(phi);
333 double dy = d*sin(phi);
335 MatrixXd
y2 = MatrixXd::Zero(track.
hits.size(), 1);
336 for(
unsigned int i=0;i<track.
hits.size();i++)
338 y2(i,0) = track.
hits[i].get_z();
343 MatrixXd
X2 = MatrixXd::Zero(track.
hits.size(), 2);
344 for(
unsigned int i=0;i<track.
hits.size();i++)
346 double D = sqrt( pow(dx - track.
hits[i].get_x(), 2) + pow(dy - track.
hits[i].get_y(),2));
352 if(v >= 0.999999){v = 0.999999;}
358 double temp2 = D*0.5;
371 if((using_vertex==
true ) && (i == (track.
hits.size() - 1)))
383 MatrixXd Xt2 = X2.transpose();
384 MatrixXd prod2 = Xt2*
X2;
385 MatrixXd inv2 = prod2.fullPivLu().inverse();
386 MatrixXd beta2 = inv2*Xt2*
y2;
388 MatrixXd diff2 = y2 - (X2*beta2);
389 MatrixXd chi2_z = (diff2.transpose())*diff2;
390 for(
unsigned int i=0;i<track.
hits.size();i++)
392 chi2_hit[i] += diff2(i,0)*diff2(i,0);
395 double z0 = beta2(1,0);
396 double dzdl = beta2(0,0)/sqrt(1. + beta2(0,0)*beta2(0,0));
414 cx = (track.
d+
r)*cos(track.
phi);
415 cy = (track.
d+
r)*sin(track.
phi);
417 double chi2_tot = 0.;
418 for(
unsigned int h=0;
h<track.
hits.size();
h++)
420 double dx1 = track.
hits[
h].get_x() - cx;
421 double dy1 = track.
hits[
h].get_y() - cy;
423 double dx2 = track.
hits[
h].get_x() + cx;
424 double dy2 = track.
hits[
h].get_y() + cy;
426 double xydiff1 = sqrt(dx1*dx1 + dy1*dy1) -
r;
427 double xydiff2 = sqrt(dx2*dx2 + dy2*dy2) -
r;
428 double xydiff = xydiff2;
429 if(fabs(xydiff1) < fabs(xydiff2)){ xydiff = xydiff1; }
433 if((using_vertex ==
true) && (
h == (track.
hits.size() - 1)))
440 chi2_hit[
h] += xydiff*xydiff/(ls_xy*ls_xy);
441 chi2_hit[
h] += diff2(
h,0)*diff2(
h,0);
444 chi2_tot += chi2_hit[
h];
447 unsigned int deg_of_freedom = 2*track.
hits.size() - 5;
449 if(using_vertex ==
true)
451 track.
hits.pop_back();
461 return (chi2_tot)/((double)(deg_of_freedom));