6 #include <HelixHough/HelixKalmanState.h>
7 #include <HelixHough/SimpleHit3D.h>
25 using namespace Eigen;
34 _hough_space(nullptr),
39 temp_combo(std::vector<unsigned
int>()),
40 combos(std::set<std::vector<unsigned
int> >()),
41 layer_sorted(std::vector<std::vector<
SimpleHit3D> >()),
44 allowed_missing_inner_hits(0),
45 ca_cos_ang_cut(0.985),
47 ca_chi2_layer_cut(2.0),
48 ca_phi_cut(
M_PI/144.),
52 fast_chi2_cut_par0(12.),
53 fast_chi2_cut_par1(0.),
56 _detector_radii(std::vector<float>()),
57 _detector_scatter(std::vector<float>()),
58 _detector_materials(std::vector<float>()),
59 _integrated_scatter(std::vector<float>()),
60 _hits_used(std::map<unsigned
int, bool>()),
66 remove_inner_hits(
false),
67 require_inner_hits(
false),
96 int CellularAutomaton_v1::run(std::vector<SimpleTrack3D>& output_tracks, std::vector<HelixKalmanState>& output_track_states, std::map<unsigned int, bool>& hits_used)
105 if(
verbose > 0) cout<<
"CellularAutomaton:: initializing..."<<endl;
107 if(
verbose > 0) cout<<code<<endl;
110 cout <<
PHWHERE <<
"::Error - Initialization failed. " << endl;
114 if(
verbose > 0) cout<<
"CellularAutomaton:: processing tracks... "<<endl;
118 cout <<
PHWHERE <<
"::Error - Processing tracks failed. " << endl;
122 if(
verbose > 0) cout<<
"CellularAutomaton:: outputting ca tracks..."<<endl;
126 cout <<
PHWHERE <<
"::Error - Outputting tracks failed. " << endl;
132 if(
verbose > 0) cout<<
"hits_used size "<< hits_used.size()<<endl;
149 cout <<
PHWHERE <<
"::Error - Hough Space is not set. " << endl;
161 cout <<
PHWHERE <<
"::Error - Detector radii are not set" << endl;
167 cout <<
PHWHERE <<
"::Error - Detector materials are not set" << endl;
176 std::vector<SimpleHit3D> one_layer;
204 for (
unsigned int i = 0; i < radii.size(); ++i) {
211 for (
unsigned int i = 0; i < materials.size(); ++i) {
213 sqrt(3. * materials[i]));
218 float total_scatter_2 = 0.;
251 for (
unsigned int i = 0; i <
ca_tracks.size(); ++i)
265 for (
unsigned int i = 0; i <
in_tracks.size(); ++i)
267 if(
verbose > 10) cout<<
"track candidate "<<i<<endl;
299 std::vector<TrackSegment> segments1;
300 std::vector<TrackSegment> segments2;
302 std::vector<TrackSegment>* cur_seg = &segments1;
303 std::vector<TrackSegment>* next_seg = &segments2;
304 unsigned int cur_seg_size = 0;
305 unsigned int next_seg_size = 0;
307 std::vector<TrackSegment> complete_segments;
311 for (
unsigned int l = 0; l < 3; ++l) {
315 if(
verbose > 10) cout<<
"track.hits.size "<<track.
hits.size()<<endl;
316 for (
unsigned int i = 0; i < track.
hits.size(); ++i) {
319 if(
verbose > 10) cout<<
"layer "<<layer<< endl;
321 if (layer > (
nlayers-1))
continue;
326 for (
unsigned int l = min; l <=
layer; ++l) {
328 if(
verbose > 10) cout<<
"adding hit in layer "<<l<<endl;
333 for (
unsigned int l = 0; l< 3; ++l){
342 float ca_cos_ang_cut_diff_inv = 1. / ca_cos_ang_cut_diff;
346 std::vector<float> inv_layer;
348 for (
unsigned int l = 3; l <
nlayers; ++l) {
349 inv_layer[l] = 1. / (((float)l) - 2.);
389 temp_segment.
hits.assign(nlayers, 0);
391 for (
unsigned int i = 0; i <
layer_sorted[0].size(); ++i) {
392 for (
unsigned int j = 0; j <
layer_sorted[1].size(); ++j) {
400 layer0 = nlayers-layer0-1;
401 layer1 = nlayers-layer1-1;
402 layer2 = nlayers-layer2-1;
409 dx1 = 0.5*sqrt(12.0)*sqrt(
layer_sorted[0][i].get_size(0,0));
410 dy1 = 0.5*sqrt(12.0)*sqrt(
layer_sorted[0][i].get_size(1,1));
411 dz1 = 0.5*sqrt(12.0)*sqrt(
layer_sorted[0][i].get_size(2,2));
417 dx2 = 0.5*sqrt(12.0)*sqrt(
layer_sorted[1][j].get_size(0,0));
418 dy2 = 0.5*sqrt(12.0)*sqrt(
layer_sorted[1][j].get_size(1,1));
419 dz2 = 0.5*sqrt(12.0)*sqrt(
layer_sorted[1][j].get_size(2,2));
433 z3, dx1, dy1, dz1, dx2, dy2, dz2,
434 dx3, dy3, dz3, kappa, dkappa,
435 ux_mid, uy_mid, ux_end, uy_end,
436 dzdl_1, dzdl_2, ddzdl_1, ddzdl_2);
439 cout<<
"Triplet : "<<endl;
440 cout<<
"kappa "<<kappa<<
" dkappa "<<dkappa<<
" ux_mid "<<ux_mid<<
" uy_mid "<<
" ux_end "<<ux_end<<
441 " uy_end " <<uy_end<<
" dzdl_1 "<<dzdl_1<<
" dzdl_2 "<<dzdl_2<<
" ddzdl_1 "<<ddzdl_1<<
" ddzdl_2 "<<ddzdl_2<<endl;
444 temp_segment.
chi2 = pow(
446 (ddzdl_1 + ddzdl_2 + fabs(dzdl_1 * ca_sin_ang_cut)),2);
449 temp_segment.
ux = ux_end;
450 temp_segment.
uy = uy_end;
451 temp_segment.
kappa = kappa;
453 temp_segment.
dkappa = dkappa;
455 temp_segment.
hits[0] = hit1;
456 temp_segment.
hits[1] = hit2;
457 temp_segment.
hits[2] = hit3;
461 unsigned int outer_layer =
463 if (!
forward) outer_layer = nlayers-outer_layer-1;
468 complete_segments.push_back(temp_segment);
470 if (next_seg->size() == next_seg_size) {
471 next_seg->push_back(temp_segment);
474 (*next_seg)[next_seg_size] = temp_segment;
482 swap(cur_seg, next_seg);
483 swap(cur_seg_size, next_seg_size);
490 for (
unsigned int i = 0; i < complete_segments.size(); ++i) {
491 if (cur_seg->size() == cur_seg_size) {
492 cur_seg->push_back(complete_segments[i]);
495 (*cur_seg)[cur_seg_size] = complete_segments[i];
500 std::set<unsigned int> comp1;
501 std::set<unsigned int> comp2;
504 if (cur_seg_size==0 || cur_seg_size>10000)
return 1;
505 for (
unsigned int i = cur_seg_size-1; i>0; --i){
506 if ((*cur_seg)[i].n_hits==0)
continue;
509 for (
unsigned int l = 0; l < (*cur_seg)[i].n_hits; ++l) {
517 (*cur_seg)[i].n_hits = 0.;
519 if (
combos.size() > 10000) {
524 for (
int j = i-1; j>=0; --j){
526 for (
unsigned int m = 0;
m < (*cur_seg)[j].n_hits; ++
m){
529 for (std::set<unsigned int>::iterator it=comp1.begin(); it!=comp1.end(); ++
it){
530 auto it2 = comp2.find(*it);
531 if (it2 != comp2.end()) comp2.erase(*it2);
534 (*cur_seg)[j].n_hits = 0;
543 unsigned int nsegs = cur_seg_size;
544 for (
unsigned int i = 0; i<cur_seg_size; i++) {
545 if ((*cur_seg)[i].n_hits ==0) --nsegs;
552 unsigned int which_seg;
557 std::map<unsigned int, unsigned int> missing_layers_map;
558 std::map<unsigned int, unsigned int> missing_layers_map_next;
559 missing_layers_map.clear();
560 missing_layers_map_next.clear();
561 for (
unsigned int n = 0 ;
n < cur_seg_size; ++
n) missing_layers_map.insert(make_pair(
n,0));
562 unsigned int added_next_segments = 0;
565 for (
unsigned int l=3; l <
nlayers; ++l ) {
570 for (
unsigned int i = 0; i < cur_seg_size; ++i) {
571 if ((*cur_seg)[i].n_hits ==0)
continue;
572 added_next_segments = 0;
573 auto search = missing_layers_map.find(i);
574 unsigned int missing_layers = search->second;
584 x1 =
layer_sorted[l - 2][(*cur_seg)[i].hits[l - 2]].get_x();
585 y1 =
layer_sorted[l - 2][(*cur_seg)[i].hits[l - 2]].get_y();
586 z1 =
layer_sorted[l - 2][(*cur_seg)[i].hits[l - 2]].get_z();
587 dx1 = 0.5*sqrt(12.0)*sqrt(
layer_sorted[l - 2][(*cur_seg)[i].
hits[l - 2]].get_size(0,0));
588 dy1 = 0.5*sqrt(12.0)*sqrt(
layer_sorted[l - 2][(*cur_seg)[i].
hits[l - 2]].get_size(1,1));
589 dz1 = 0.5*sqrt(12.0)*sqrt(
layer_sorted[l - 2][(*cur_seg)[i].
hits[l - 2]].get_size(2,2));
595 x1 = clusterl2.
get_x();
596 y1 = clusterl2.
get_y();
597 z1 = clusterl2.
get_z();
598 dx1 = 0.5*sqrt(12.0)*sqrt(clusterl2.
get_size(0,0));
599 dy1 = 0.5*sqrt(12.0)*sqrt(clusterl2.
get_size(1,1));
600 dz1 = 0.5*sqrt(12.0)*sqrt(clusterl2.
get_size(2,2));
604 x2 =
layer_sorted[l - 1][(*cur_seg)[i].hits[l - 1]].get_x();
605 y2 =
layer_sorted[l - 1][(*cur_seg)[i].hits[l - 1]].get_y();
606 z2 =
layer_sorted[l - 1][(*cur_seg)[i].hits[l - 1]].get_z();
607 dx2 = 0.5*sqrt(12.0)*sqrt(
layer_sorted[l - 1][(*cur_seg)[i].
hits[l - 1]].get_size(0,0));
608 dy2 = 0.5*sqrt(12.0)*sqrt(
layer_sorted[l - 1][(*cur_seg)[i].
hits[l - 1]].get_size(1,1));
609 dz2 = 0.5*sqrt(12.0)*sqrt(
layer_sorted[l - 1][(*cur_seg)[i].
hits[l - 1]].get_size(2,2));
614 x2 = clusterl1.
get_x();
615 y2 = clusterl1.
get_y();
616 z2 = clusterl1.
get_z();
617 dx2 = 0.5*sqrt(12.0)*sqrt(clusterl1.
get_size(0,0));
618 dy2 = 0.5*sqrt(12.0)*sqrt(clusterl1.
get_size(1,1));
619 dz2 = 0.5*sqrt(12.0)*sqrt(clusterl1.
get_size(2,2));
626 bool fit_layer = (l >= 6);
631 for (
unsigned int ll = 0; ll < (*cur_seg)[i].n_hits; ++ll) {
637 init_track.
hits[ll] = cluster;
642 for (std::map<unsigned int,SimpleHit3D>::iterator jt =
_hits_map.begin();
648 if (layer != l)
continue;
658 float phi_diff = phi_cur-phi_prev;
659 if (phi_cur< M_PI/2. && phi_prev > 3*
M_PI/2.) phi_diff += 2.*
M_PI;
660 else if (phi_cur>3*
M_PI/2 && phi_prev<
M_PI/2.) phi_diff -= 2.*
M_PI;
677 for (
unsigned int ll = 0; ll < init_track.
hits.size(); ++ll) {
678 temp_track.
hits[ll] = init_track.
hits[ll];
680 temp_track.
hits[init_track.
hits.size()] = hit3d;
684 float temp_chi2 = temp_track.
fit_track();
686 if (temp_chi2 != temp_chi2)
continue;
687 if (temp_track.
kappa != temp_track.
kappa )
continue;
688 if (temp_track.
z0 != temp_track.
z0)
continue;
691 state.
phi = temp_track.
phi;
692 if (state.
phi < 0.) {
695 state.
d = temp_track.
d;
698 state.
z0 = temp_track.
z0;
700 state.
C = Matrix<float, 5, 5>::Zero(5, 5);
701 state.
C(0, 0) = pow(0.01, 2.);
702 state.
C(1, 1) = pow(0.01, 2.);
703 state.
C(2, 2) = pow(0.01 * state.
nu, 2.);
704 state.
C(3, 3) = pow(0.05, 2.);
705 state.
C(4, 4) = pow(0.05, 2.);
736 for (
unsigned int ll = 0; ll < l; ++ll) {
737 temp_segment.
hits[ll] = (*cur_seg)[which_seg].hits[ll];
739 temp_segment.
hits[l] = hit1;
740 temp_segment.
n_hits = l + 1;
742 if (next_seg->size() == next_seg_size) {
743 next_seg->push_back(temp_segment);
744 missing_layers_map_next.insert(make_pair(next_seg->size()-1, missing_layers));
748 cout<<
"Next segment size inconsistent "<<endl;
750 (*next_seg)[next_seg_size] = temp_segment;
751 missing_layers_map_next.insert(make_pair(next_seg->size()-1, missing_layers));
754 ++added_next_segments;
756 cout<<
"segment "<< which_seg <<
" added segment "<<added_next_segments<<endl;
778 dx3 = 0.5*sqrt(12.0)*sqrt(hit3d.
get_size(0,0));
779 dy3 = 0.5*sqrt(12.0)*sqrt(hit3d.
get_size(1,1));
780 dz3 = 0.5*sqrt(12.0)*sqrt(hit3d.
get_size(2,2));
782 cur_kappa = (*cur_seg)[i].kappa;
783 cur_dkappa = (*cur_seg)[i].dkappa;
784 cur_ux = (*cur_seg)[i].ux;
785 cur_uy = (*cur_seg)[i].uy;
786 cur_chi2 = (*cur_seg)[i].chi2;
790 x1, y1, z1, x2, y2, z2, x3, y3, z3,
791 dx1, dy1, dz1, dx2, dy2, dz2, dx3, dy3, dz3,
792 kappa, dkappa, ux_mid, uy_mid, ux_end, uy_end,
793 dzdl_1, dzdl_2, ddzdl_1, ddzdl_2,
794 ca_sin_ang_cut, ca_cos_ang_cut_diff_inv,
795 cur_kappa, cur_dkappa, cur_ux, cur_uy, cur_chi2, chi2);
797 cout<<
"Extended layers for segment "<<which_seg<<endl;
798 cout<<
"kappa "<<kappa<<
" dkappa "<<dkappa
799 <<
" ux_mid "<<ux_mid<<
" uy_mid "<<
" ux_end "<<ux_end<<
" uy_end " <<uy_end
800 <<
" dzdl_1 "<<dzdl_1<<
" dzdl_2 "<<dzdl_2<<
" ddzdl_1 "<<ddzdl_1<<
" ddzdl_2 "<<ddzdl_2
801 <<
" cur_chi2 "<<cur_chi2<<
" chi2 "<<chi2<<
" chi2*inv_layer "<<l <<
" "<<chi2*inv_layer[l]
808 temp_segment.
chi2 = chi2;
809 temp_segment.
ux = ux_end;
810 temp_segment.
uy = uy_end;
811 temp_segment.
kappa = kappa;
818 temp_segment.
dkappa = dkappa;
819 for (
unsigned int ll = 0; ll < l; ++ll) {
820 temp_segment.
hits[ll] = (*cur_seg)[which_seg].hits[ll];
822 temp_segment.
hits[l] = hit1;
826 temp_segment.
n_hits = l + 1;
837 if (next_seg->size() == next_seg_size) {
838 next_seg->push_back(temp_segment);
839 missing_layers_map_next.insert(make_pair(next_seg->size()-1, missing_layers));
843 cout<<
"Next segment size inconsistent "<<endl;
845 (*next_seg)[next_seg_size] = temp_segment;
846 missing_layers_map_next.insert(make_pair(next_seg->size()-1, missing_layers));
849 ++added_next_segments;
851 cout<<
"segment "<< which_seg <<
" added segment "<<added_next_segments<<endl;
877 swap(cur_seg, next_seg);
878 swap(cur_seg_size, next_seg_size);
879 missing_layers_map.swap(missing_layers_map_next);
890 std::vector<SimpleTrack3D> best_track;
891 std::vector<HelixKalmanState> best_track_state;
892 float best_chi2 = 9999;
893 for (
unsigned int i = 0; i< cur_seg_size; ++i) {
895 if ((*cur_seg)[i].n_hits ==0)
continue;
898 cout<<
"segment " <<i <<endl;
903 for (
unsigned int l = 0; l < (*cur_seg)[i].n_hits; ++l) {
909 temp_track.
hits[l] = cluster;
913 float init_chi2 = temp_track.
fit_track();
914 if(
verbose > 10) cout<<
"chi2 from fit_track "<<init_chi2 <<
" kappa "<< temp_track.
kappa <<endl;
917 cout <<
" kappa " <<temp_track.
kappa <<
" phi "<<temp_track.
phi<<
" d "<<temp_track.
d
918 <<
" z0 "<<temp_track.
z0<<
" dzdl "<<temp_track.
dzdl<< endl;
922 if (temp_track.
kappa != temp_track.
kappa && cur_seg_size!=1)
continue;
923 if (temp_track.
z0 != temp_track.
z0 && cur_seg_size != 1)
continue;
925 if (temp_track.
kappa != temp_track.
kappa)
continue;
926 if (temp_track.
z0 != temp_track.
z0)
continue;
930 state.
phi = temp_track.
phi;
931 if (state.
phi < 0.) {
934 state.
d = temp_track.
d;
937 state.
z0 = temp_track.
z0;
939 state.
C = Matrix<float, 5, 5>::Zero(5, 5);
940 state.
C(0, 0) = pow(0.01, 2.);
941 state.
C(1, 1) = pow(0.01, 2.);
942 state.
C(2, 2) = pow(0.01 * state.
nu, 2.);
943 state.
C(3, 3) = pow(0.05, 2.);
944 state.
C(4, 4) = pow(0.05, 2.);
951 unsigned int nfits = 0;
952 for (
unsigned int h = 0;
h < temp_track.
hits.size(); ++
h) {
956 cout<<
"nfits "<<nfits<<endl;
960 cout<<
"z0 after kalman "<<state.
z0<<endl;
967 if (!(temp_track.
kappa == temp_track.
kappa) && (cur_seg_size !=1))
continue;
968 if (!(state.
chi2 == state.
chi2) && (cur_seg_size !=1))
continue;
970 if (!(temp_track.
kappa == temp_track.
kappa))
continue;
971 if (!(state.
chi2 == state.
chi2))
continue;
983 if (state.
chi2 / (2. * ((
float)(temp_track.
hits.size())) - 5.) >
ca_chi2_cut && cur_seg_size !=1)
continue;
989 if (!best_track.empty()){
990 best_track.pop_back();
991 best_track_state.pop_back();
993 best_track.push_back(temp_track);
994 best_track_state.push_back(state);
998 if (best_track.empty())
return 1;
1000 if(
verbose > 10) cout<<
"best_track.size "<<best_track.size()<<endl;
1003 if(
verbose > 10) cout <<
"ca track added, chi2 = "<< (best_track_state.back().chi2)/(2. * ((
float)(temp_track.
hits.size())) - 5.) <<
" z0 = "<<best_track_state.back().z0<<endl;
1008 temp_track = best_track.back();
1011 for (
unsigned int i = 0; i < temp_track.
hits.size(); ++i) {
1029 std::vector<TrackSegment> segments1;
1030 std::vector<TrackSegment> segments2;
1033 std::vector<TrackSegment>* cur_seg = &segments1;
1034 std::vector<TrackSegment>* next_seg = &segments2;
1035 unsigned int cur_seg_size = 0;
1036 unsigned int next_seg_size = 0;
1038 std::vector<TrackSegment> complete_segments;
1041 cout<<
"allowed missing "<< allowed_missing<<endl;
1045 for (
unsigned int l = 0; l <
nlayers; ++l) {
1050 for (
unsigned int i = 0; i < track.
hits.size(); ++i) {
1053 if (layer > (nlayers-1))
continue;
1054 if (!
forward) layer = nlayers-layer-1;
1055 unsigned int min = (layer - allowed_missing);
1056 if (allowed_missing > layer) {
1059 for (
unsigned int l = min; l <=
layer; ++l) {
1065 for (
unsigned int l = 0; l <
nlayers; ++l) {
1076 gettimeofday(&t1,
nullptr);
1079 float ca_cos_ang_cut_diff_inv = 1. / ca_cos_ang_cut_diff;
1082 std::vector<float> inv_layer;
1083 inv_layer.assign(nlayers, 1.);
1084 for (
unsigned int l = 3; l <
nlayers; ++l) {
1085 inv_layer[l] = 1. / (((float)l) - 2.);
1091 float dx1, dx2, dx3;
1092 float dy1, dy2, dy3;
1093 float dz1,
dz2, dz3;
1121 temp_segment.
hits.assign(nlayers, 0);
1123 for (
unsigned int i = 0; i <
layer_sorted[0].size(); ++i) {
1124 for (
unsigned int j = 0; j <
layer_sorted[1].size(); ++j) {
1132 layer0 = nlayers-layer0-1;
1133 layer1 = nlayers-layer1-1;
1134 layer2 = nlayers-layer2-1;
1136 if ((layer0 >= layer1) || (layer1 >= layer2)) {
1145 dx1 = 0.5*sqrt(12.0)*sqrt(
layer_sorted[0][i].get_size(0,0));
1146 dy1 = 0.5*sqrt(12.0)*sqrt(
layer_sorted[0][i].get_size(1,1));
1147 dz1 = 0.5*sqrt(12.0)*sqrt(
layer_sorted[0][i].get_size(2,2));
1153 dx2 = 0.5*sqrt(12.0)*sqrt(
layer_sorted[1][j].get_size(0,0));
1154 dy2 = 0.5*sqrt(12.0)*sqrt(
layer_sorted[1][j].get_size(1,1));
1155 dz2 = 0.5*sqrt(12.0)*sqrt(
layer_sorted[1][j].get_size(2,2));
1160 dx3 = 0.5*sqrt(12.0)*sqrt(
layer_sorted[2][
k].get_size(0,0));
1161 dy3 = 0.5*sqrt(12.0)*sqrt(
layer_sorted[2][
k].get_size(1,1));
1162 dz3 = 0.5*sqrt(12.0)*sqrt(
layer_sorted[2][
k].get_size(2,2));
1170 z3, dx1, dy1, dz1, dx2, dy2, dz2,
1171 dx3, dy3, dz3, kappa, dkappa,
1172 ux_mid, uy_mid, ux_end, uy_end,
1173 dzdl_1, dzdl_2, ddzdl_1, ddzdl_2);
1176 cout<<
"Triplet : "<<endl;
1177 cout<<
"kappa "<<kappa<<
" dkappa "<<dkappa<<
" ux_mid "<<ux_mid<<
" uy_mid "<<
" ux_end "<<ux_end<<
1178 " uy_end " <<uy_end<<
" dzdl_1 "<<dzdl_1<<
" dzdl_2 "<<dzdl_2<<
" ddzdl_1 "<<ddzdl_1<<
" ddzdl_2 "<<ddzdl_2<<endl;
1181 temp_segment.
chi2 = pow(
1183 (ddzdl_1 + ddzdl_2 + fabs(dzdl_1 * ca_sin_ang_cut)),2);
1185 temp_segment.
ux = ux_end;
1186 temp_segment.
uy = uy_end;
1187 temp_segment.
kappa = kappa;
1189 temp_segment.
dkappa = dkappa;
1190 temp_segment.
hits[0] = hit1;
1191 temp_segment.
hits[1] = hit2;
1192 temp_segment.
hits[2] = hit3;
1194 unsigned int outer_layer =
1196 if (!
forward) outer_layer = nlayers-outer_layer-1;
1198 if ((outer_layer - 2) > allowed_missing)
continue;
1200 if ((nlayers - 3) <= allowed_missing) {
1201 complete_segments.push_back(temp_segment);
1203 if (next_seg->size() == next_seg_size) {
1204 next_seg->push_back(temp_segment);
1207 (*next_seg)[next_seg_size] = temp_segment;
1215 swap(cur_seg, next_seg);
1216 swap(cur_seg_size, next_seg_size);
1219 unsigned int which_seg;
1222 for (
unsigned int l = 3; l <
nlayers; ++l) {
1223 if (l == (nlayers - 1)) {
1227 for (
unsigned int i = 0; i < cur_seg_size; ++i) {
1228 for (
unsigned int j = 0; j <
layer_sorted[l].size(); ++j) {
1230 unsigned int layer0 =
layer_sorted[l - 1][(*cur_seg)[i].hits[l - 1]].get_layer();
1234 layer0 = nlayers-layer0-1;
1235 layer1 = nlayers-layer1-1;
1237 if (layer0 >= layer1)
continue;
1239 x1 =
layer_sorted[l - 2][(*cur_seg)[i].hits[l - 2]].get_x();
1240 y1 =
layer_sorted[l - 2][(*cur_seg)[i].hits[l - 2]].get_y();
1241 z1 =
layer_sorted[l - 2][(*cur_seg)[i].hits[l - 2]].get_z();
1242 x2 =
layer_sorted[l - 1][(*cur_seg)[i].hits[l - 1]].get_x();
1243 y2 =
layer_sorted[l - 1][(*cur_seg)[i].hits[l - 1]].get_y();
1244 z2 =
layer_sorted[l - 1][(*cur_seg)[i].hits[l - 1]].get_z();
1249 dx1 = 0.5*sqrt(12.0)*sqrt(
layer_sorted[l - 2][(*cur_seg)[i].
hits[l - 2]].get_size(0,0));
1250 dy1 = 0.5*sqrt(12.0)*sqrt(
layer_sorted[l - 2][(*cur_seg)[i].
hits[l - 2]].get_size(1,1));
1251 dz1 = 0.5*sqrt(12.0)*sqrt(
layer_sorted[l - 2][(*cur_seg)[i].
hits[l - 2]].get_size(2,2));
1252 dx2 = 0.5*sqrt(12.0)*sqrt(
layer_sorted[l - 1][(*cur_seg)[i].
hits[l - 1]].get_size(0,0));
1253 dy2 = 0.5*sqrt(12.0)*sqrt(
layer_sorted[l - 1][(*cur_seg)[i].
hits[l - 1]].get_size(1,1));
1254 dz2 = 0.5*sqrt(12.0)*sqrt(
layer_sorted[l - 1][(*cur_seg)[i].
hits[l - 1]].get_size(2,2));
1255 dx3 = 0.5*sqrt(12.0)*sqrt(
layer_sorted[l][j].get_size(0,0));
1256 dy3 = 0.5*sqrt(12.0)*sqrt(
layer_sorted[l][j].get_size(1,1));
1257 dz3 = 0.5*sqrt(12.0)*sqrt(
layer_sorted[l][j].get_size(2,2));
1259 cur_kappa = (*cur_seg)[i].kappa;
1260 cur_dkappa = (*cur_seg)[i].dkappa;
1261 cur_ux = (*cur_seg)[i].ux;
1262 cur_uy = (*cur_seg)[i].uy;
1263 cur_chi2 = (*cur_seg)[i].chi2;
1269 x1, y1, z1, x2, y2, z2, x3, y3, z3,
1270 dx1, dy1, dz1, dx2, dy2, dz2, dx3, dy3, dz3,
1271 kappa, dkappa, ux_mid, uy_mid, ux_end, uy_end,
1272 dzdl_1, dzdl_2, ddzdl_1, ddzdl_2,
1273 ca_sin_ang_cut, ca_cos_ang_cut_diff_inv,
1274 cur_kappa, cur_dkappa, cur_ux, cur_uy, cur_chi2, chi2);
1277 cout<<
"Extended layers for segment "<<which_seg<<endl;
1278 cout<<
"kappa "<<kappa<<
" dkappa "<<dkappa
1279 <<
" ux_mid "<<ux_mid<<
" uy_mid "<<
" ux_end "<<ux_end<<
" uy_end " <<uy_end
1280 <<
" dzdl_1 "<<dzdl_1<<
" dzdl_2 "<<dzdl_2<<
" ddzdl_1 "<<ddzdl_1<<
" ddzdl_2 "<<ddzdl_2
1281 <<
" cur_chi2 "<<cur_chi2<<
" chi2 "<<chi2<<
" chi2*inv_layer "<<l <<
" "<<chi2*inv_layer[l]
1285 temp_segment.
chi2 = chi2;
1286 temp_segment.
ux = ux_end;
1287 temp_segment.
uy = uy_end;
1288 temp_segment.
kappa = kappa;
1290 temp_segment.
dkappa = dkappa;
1291 for (
unsigned int ll = 0; ll < l; ++ll) {
1292 temp_segment.
hits[ll] = (*cur_seg)[which_seg].hits[ll];
1294 temp_segment.
hits[l] = hit1;
1295 unsigned int outer_layer =
1297 if (!
forward) outer_layer = nlayers-outer_layer-1;
1298 temp_segment.
n_hits = l + 1;
1300 if ((nlayers - (l + 1)) <= allowed_missing) {
1301 complete_segments.push_back(temp_segment);
1304 if ((outer_layer - l) > allowed_missing) {
1307 if (next_seg->size() == next_seg_size) {
1308 next_seg->push_back(temp_segment);
1311 (*next_seg)[next_seg_size] = temp_segment;
1318 swap(cur_seg, next_seg);
1319 swap(cur_seg_size, next_seg_size);
1327 for (
unsigned int i = 0; i < complete_segments.size(); ++i) {
1328 if (cur_seg->size() == cur_seg_size) {
1329 cur_seg->push_back(complete_segments[i]);
1332 (*cur_seg)[cur_seg_size] = complete_segments[i];
1337 gettimeofday(&t2,
nullptr);
1338 time1 = ((double)(t1.tv_sec) + (double)(t1.tv_usec) / 1000000.);
1339 time2 = ((double)(t2.tv_sec) + (double)(t2.tv_usec) / 1000000.);
1340 CAtime += (time2 - time1);
1342 std::set<unsigned int> comp1;
1343 std::set<unsigned int> comp2;
1346 if (cur_seg_size==0 || cur_seg_size>10000)
return 1;
1347 for (
unsigned int i = cur_seg_size-1; i>0; --i){
1348 if ((*cur_seg)[i].n_hits==0)
continue;
1351 for (
unsigned int l = 0; l < (*cur_seg)[i].n_hits; ++l) {
1358 if (it !=
combos.end()) {
1359 (*cur_seg)[i].n_hits = 0.;
1363 if (
combos.size() > 100000) {
1368 for (
int j = i-1; j>=0; --j){
1370 for (
unsigned int m = 0;
m < (*cur_seg)[j].n_hits; ++
m){
1373 for (std::set<unsigned int>::iterator it=comp1.begin(); it!=comp1.end(); ++
it){
1374 auto it2 = comp2.find(*it);
1375 if (it2 != comp2.end()) comp2.erase(*it2);
1377 if (comp2.empty()) {
1378 (*cur_seg)[j].n_hits = 0;
1384 unsigned int nsegs = cur_seg_size;
1385 for (
unsigned int i = 0; i<cur_seg_size; i++) {
1386 if ((*cur_seg)[i].n_hits ==0) --nsegs;
1393 std::vector<SimpleTrack3D> best_track;
1394 std::vector<HelixKalmanState> best_track_state;
1395 float best_chi2 = 9999;
1396 for (
unsigned int i = 0; i< cur_seg_size; ++i) {
1398 if ((*cur_seg)[i].n_hits ==0)
continue;
1401 cout<<
"segment " <<i <<endl;
1405 for (
unsigned int l = 0; l < (*cur_seg)[i].n_hits; ++l) {
1409 unsigned int ninner_hits =0;
1410 for (
unsigned int n = 0;
n < temp_track.
hits.size(); ++
n){
1411 if (temp_track.
hits[
n].get_layer()<3)
1417 gettimeofday(&t1,
nullptr);
1419 float init_chi2 = temp_track.
fit_track();
1421 cout<<
"chi2 from fit_track "<<init_chi2
1422 <<
" kappa " <<temp_track.
kappa <<
" phi "<<temp_track.
phi<<
" d "<<temp_track.
d
1423 <<
" z0 "<<temp_track.
z0<<
" dzdl "<<temp_track.
dzdl<< endl;
1430 gettimeofday(&t2,
nullptr);
1431 time1 = ((double)(t1.tv_sec) + (double)(t1.tv_usec) / 1000000.);
1432 time2 = ((double)(t2.tv_sec) + (double)(t2.tv_usec) / 1000000.);
1439 state.
phi = temp_track.
phi;
1440 if (state.
phi < 0.) {
1443 state.
d = temp_track.
d;
1446 state.
z0 = temp_track.
z0;
1448 state.
C = Matrix<float, 5, 5>::Zero(5, 5);
1449 state.
C(0, 0) = pow(0.01, 2.);
1450 state.
C(1, 1) = pow(0.01, 2.);
1451 state.
C(2, 2) = pow(0.01 * state.
nu, 2.);
1452 state.
C(3, 3) = pow(0.05, 2.);
1453 state.
C(4, 4) = pow(0.05, 2.);
1460 unsigned int nfits = 0;
1461 for (
unsigned int h = 0;
h < temp_track.
hits.size(); ++
h) {
1465 if(
verbose > 0) cout<<
"z0 after kalman "<<state.
z0<<endl;
1471 gettimeofday(&t2,
nullptr);
1472 time1 = ((double)(t1.tv_sec) + (double)(t1.tv_usec) / 1000000.);
1473 time2 = ((double)(t2.tv_sec) + (double)(t2.tv_usec) / 1000000.);
1476 if (!(temp_track.
kappa == temp_track.
kappa)) {
1500 if (best_chi2 > state.
chi2){
1501 if (!best_track.empty()){
1502 best_track.pop_back();
1503 best_track_state.pop_back();
1505 best_track.push_back(temp_track);
1506 best_track_state.push_back(state);
1510 if (best_track.empty())
return 1;
1514 if(
verbose > 0) cout <<
"ca track added, chi2 = "<< (best_track_state.back().chi2)/(2. * ((
float)(temp_track.
hits.size())) - 5.) <<
" z0 = "<<best_track_state.back().z0<<endl;
1519 temp_track = best_track.back();
1522 for (
unsigned int i = 0; i < temp_track.
hits.size(); ++i) {
1540 float x1,
float y1,
float z1,
float x2,
float y2,
float z2,
1541 float x3,
float y3,
float z3,
1542 float dx1,
float dy1,
float dz1,
float dx2,
float dy2,
float dz2,
1543 float dx3,
float dy3,
float dz3,
1544 float& kappa,
float& dkappa,
1545 float& ux_mid,
float& uy_mid,
float& ux_end,
float& uy_end,
1546 float& dzdl_1,
float& dzdl_2,
float& ddzdl_1,
float& ddzdl_2)
1549 float D12 = sqrt(pow(x2-x1,2)+pow(y2-y1,2));
1550 float D23 = sqrt(pow(x3-x2,2)+pow(y3-y2,2));
1551 float D13 = sqrt(pow(x3-x1,2)+pow(y3-y1,2));
1552 kappa = 1./(D12*D23*D13);
1553 float num = (D12+D23+D13)*(D23+D13-D12)*(D12+D13-D23)*(D12+D23-D13);
1558 float kappa_inv = 1/kappa;
1559 float D12_inv = 1./D12;
1560 float D23_inv = 1./D23;
1561 float D13_inv = 1./D13;
1563 float dr1 = sqrt(pow(dx1,2)+pow(dy1,2));
1564 float dr2 = sqrt(pow(dx2,2)+pow(dy2,2));
1565 float dr3 = sqrt(pow(dx3,2)+pow(dy3,2));
1567 float dk1 = (dr1+dr2)* D12_inv*D12_inv;
1568 float dk2 = (dr2+dr3)* D23_inv*D23_inv;
1571 float ux12 = (x2-
x1)*D12_inv;
1572 float uy12 = (y2-
y1)*D12_inv;
1573 float ux23 = (x3-
x2)*D23_inv;
1574 float uy23 = (y3-
y2)*D23_inv;
1575 float ux13 = (x3-
x1)*D13_inv;
1576 float uy13 = (y3-
y1)*D13_inv;
1580 float cosalpha = ux12*ux13 + uy12*uy13;
1581 float sinalpha = uy12*ux13 - ux12*uy13;
1583 ux_mid = ux23 * cosalpha - uy23 * sinalpha;
1584 uy_mid = ux23 * sinalpha + uy23 * cosalpha;
1586 ux_end = ux23 * cosalpha + uy23 * sinalpha;
1587 uy_end = uy23 * cosalpha - ux23 * sinalpha;
1590 float ds23 = 2.*kappa_inv*atan(sinalpha/(1.+ sqrt(1.-pow(sinalpha,2))));
1591 if (kappa<=0) ds23 = D23;
1593 float dz23 = z3 -
z2;
1594 dzdl_2 = dz23/sqrt(pow(ds23,2) + pow(dz23,2));
1595 ddzdl_2 = (dz2 + dz3)*D23_inv;
1598 sinalpha = ux13 *uy23 - ux23 * uy13;
1599 float ds12 = 2.*kappa_inv*atan(sinalpha/(1.+sqrt(1.-pow(sinalpha,2))));
1600 if (kappa<=0) ds12 = D12;
1602 float dz12 = z2 -
z1;
1603 dzdl_1 = dz12/sqrt(pow(ds12,2) + pow(dz12,2));
1604 ddzdl_1 = (dz1 +
dz2) * D12_inv;
1611 float x1,
float y1,
float z1,
float x2,
float y2,
float z2,
1612 float x3,
float y3,
float z3,
1613 float dx1,
float dy1,
float dz1,
float dx2,
float dy2,
float dz2,
1614 float dx3,
float dy3,
float dz3,
1615 float& kappa,
float& dkappa,
1616 float& ux_mid,
float& uy_mid,
float& ux_end,
float& uy_end,
1617 float& dzdl_1,
float& dzdl_2,
float& ddzdl_1,
float& ddzdl_2,
1618 float ca_sin_ang_cut,
float ca_cos_ang_cut_diff_inv,
1619 float cur_kappa,
float cur_dkappa,
float cur_ux,
float cur_uy,
1620 float cur_chi2,
float& chi2)
1624 float D12 = sqrt(pow(x2-x1,2)+pow(y2-y1,2));
1625 float D23 = sqrt(pow(x3-x2,2)+pow(y3-y2,2));
1626 float D13 = sqrt(pow(x3-x1,2)+pow(y3-y1,2));
1627 kappa = 1./(D12*D23*D13);
1628 float num = (D12+D23+D13)*(D23+D13-D12)*(D12+D13-D23)*(D12+D23-D13);
1633 float kappa_inv = 1/kappa;
1634 float D12_inv = 1./D12;
1635 float D23_inv = 1./D23;
1636 float D13_inv = 1./D13;
1638 float dr1 = sqrt(pow(dx1,2)+pow(dy1,2));
1639 float dr2 = sqrt(pow(dx2,2)+pow(dy2,2));
1640 float dr3 = sqrt(pow(dx3,2)+pow(dy3,2));
1642 float dk1 = (dr1+dr2)* D12_inv*D12_inv;
1643 float dk2 = (dr2+dr3)* D23_inv*D23_inv;
1646 float ux12 = (x2-
x1)*D12_inv;
1647 float uy12 = (y2-
y1)*D12_inv;
1648 float ux23 = (x3-
x2)*D23_inv;
1649 float uy23 = (y3-
y2)*D23_inv;
1650 float ux13 = (x3-
x1)*D13_inv;
1651 float uy13 = (y3-
y1)*D13_inv;
1655 float cosalpha = ux12*ux13 + uy12*uy13;
1656 float sinalpha = uy12*ux13 - ux12*uy13;
1658 ux_mid = ux23 * cosalpha - uy23 * sinalpha;
1659 uy_mid = ux23 * sinalpha + uy23 * cosalpha;
1661 ux_end = ux23 * cosalpha + uy23 * sinalpha;
1662 uy_end = uy23 * cosalpha - ux23 * sinalpha;
1665 float ds23 = 2.*kappa_inv*atan(sinalpha/(1.+ sqrt(1.-pow(sinalpha,2))));
1666 if (kappa<=0) ds23 = D23;
1668 float dz23 = z3 -
z2;
1669 dzdl_2 = dz23/sqrt(pow(ds23,2) + pow(dz23,2));
1670 ddzdl_2 = (dz2 + dz3)*D23_inv;
1673 sinalpha = ux13 *uy23 - ux23 * uy13;
1674 float ds12 = 2.*kappa_inv*atan(sinalpha/(1.+sqrt(1.-pow(sinalpha,2))));
1675 if (kappa<=0) ds12 = D12;
1677 float dz12 = z2 -
z1;
1678 dzdl_1 = dz12/sqrt(pow(ds12,2) + pow(dz12,2));
1679 ddzdl_1 = (dz1 +
dz2) * D12_inv;
1681 float kappa_diff = cur_kappa - kappa;
1682 float n_dk = cur_dkappa + dkappa + ca_sin_ang_cut * kappa;
1683 float chi2_kappa = pow(kappa_diff,2)/pow(n_dk,2);
1685 float cos_scatter = cur_ux * ux_mid + cur_uy * uy_mid;
1686 float chi2_ang = pow((1-cos_scatter)*ca_cos_ang_cut_diff_inv,2);
1688 float sin_scatter = dzdl_1 * ca_sin_ang_cut;
1689 float chi2_dzdl = 0.5*pow((dzdl_1-dzdl_2)/(ddzdl_1+ddzdl_2+fabs(sin_scatter)),2);
1691 chi2 = cur_chi2 + chi2_ang + chi2_kappa + chi2_dzdl;
1702 if (_phi < 0.) _phi += 2.*
M_PI;