37 #include <Eigen/Dense>
92 for(
unsigned int ivtx = 0; ivtx < connected_tracks.size(); ++ivtx)
94 if(
Verbosity() > 1) std::cout <<
"process vertex " << ivtx << std::endl;
96 for(
auto it : connected_tracks[ivtx])
100 if(
Verbosity() > 2) std::cout <<
" adding track " <<
id <<
" to vertex " << ivtx << std::endl;
107 if(
Verbosity() > 1) std::cout <<
" vertex " <<
it.first <<
" track " <<
it.second << std::endl;
120 auto ret = _vertex_track_map.equal_range(
it);
121 for (
auto cit=ret.first; cit!=ret.second; ++cit)
123 unsigned int trid = cit->second;
126 for(
int i = 0; i < 3; ++i)
127 for(
int j = 0; j < 3; ++j)
128 cov(i,j) =
track->get_error(i,j);
134 avgCov /= sqrt(cov_wt);
137 std::cout <<
"Average covariance for vertex " <<
it <<
" is:" << std::endl;
138 std::cout << std::setprecision(8) << avgCov << std::endl;
145 if(_vertex_track_map.size() > 0)
148 for(
auto it : _vertex_set)
150 auto svtxVertex = std::make_unique<SvtxVertex_v1>();
152 svtxVertex->set_chisq(0.0);
153 svtxVertex->set_ndof(0);
154 svtxVertex->set_t0(0);
155 svtxVertex->set_id(
it);
157 auto ret = _vertex_track_map.equal_range(
it);
158 for (
auto cit=ret.first; cit!=ret.second; ++cit)
160 unsigned int trid = cit->second;
162 if(
Verbosity() > 1) std::cout <<
" vertex " <<
it <<
" insert track " << trid << std::endl;
163 svtxVertex->insert_track(trid);
168 svtxVertex->set_x(pos.x());
169 svtxVertex->set_y(pos.y());
170 svtxVertex->set_z(pos.z());
171 if(
Verbosity() > 1) std::cout <<
" vertex " <<
it <<
" insert pos.x " << pos.x() <<
" pos.y " << pos.y() <<
" pos.z " << pos.z() << std::endl;
174 for(
int i = 0; i < 3; ++i)
176 for(
int j = 0; j < 3; ++j)
178 svtxVertex->set_error(i, j, vtxCov(i,j));
192 auto vtxid =
track->get_vertex_id();
203 float dz =
track->get_z() - vertex->get_z();
211 track->set_vertex_id(newvtxid);
233 std::cerr <<
"DST Node missing, quitting" << std::endl;
234 throw std::runtime_error(
"failed to find DST node in PHActsInitialVertexFinder::createNodes");
246 _svtx_vertex_map = findNode::getClass<SvtxVertexMap>(topNode,
"SvtxVertexMap");
263 _track_map = findNode::getClass<SvtxTrackMap>(topNode,
"SvtxTrackMap");
266 std::cout <<
PHWHERE <<
" ERROR: Can't find SvtxTrackMap: " << std::endl;
280 auto id1 = tr1_it->first;
281 auto tr1 = tr1_it->second;
282 if(tr1->get_quality() >
_qual_cut)
continue;
285 unsigned int nmvtx = 0;
286 for(
auto clusit = tr1->begin_cluster_keys(); clusit != tr1->end_cluster_keys(); ++clusit)
295 if(
Verbosity() > 3) std::cout <<
" tr1 has nmvtx at least " << nmvtx << std::endl;
299 for(
auto tr2_it = std::next(tr1_it); tr2_it !=
_track_map->
end(); ++tr2_it)
301 auto id2 = tr2_it->first;
302 auto tr2 = tr2_it->second;
303 if(tr2->get_quality() >
_qual_cut)
continue;
306 unsigned int nmvtx = 0;
307 for(
auto clusit = tr2->begin_cluster_keys(); clusit != tr2->end_cluster_keys(); ++clusit)
316 if(
Verbosity() > 3) std::cout <<
" tr2 has nmvtx at least " << nmvtx << std::endl;
320 if(
Verbosity() > 3) std::cout <<
"Check DCA for tracks " << id1 <<
" and " << id2 << std::endl;
335 unsigned int id1 = tr1->
get_id();
336 unsigned int id2 = tr2->
get_id();
345 Eigen::Vector3d PCA1(0,0,0);
346 Eigen::Vector3d PCA2(0,0,0);
347 double dca =
dcaTwoLines(a1, b1, a2, b2, PCA1, PCA2);
354 std::cout <<
" good match for tracks " << tr1->
get_id() <<
" and " << tr2->
get_id() <<
" with pT " << tr1->
get_pt() <<
" and " << tr2->
get_pt() << std::endl;
355 std::cout <<
" a1.x " << a1.x() <<
" a1.y " << a1.y() <<
" a1.z " << a1.z() << std::endl;
356 std::cout <<
" a2.x " << a2.x() <<
" a2.y " << a2.y() <<
" a2.z " << a2.z() << std::endl;
357 std::cout <<
" PCA1.x() " << PCA1.x() <<
" PCA1.y " << PCA1.y() <<
" PCA1.z " << PCA1.z() << std::endl;
358 std::cout <<
" PCA2.x() " << PCA2.x() <<
" PCA2.y " << PCA2.y() <<
" PCA2.z " << PCA2.z() << std::endl;
359 std::cout <<
" dca " << dca << std::endl;
364 _track_pair_pca_map.insert( std::make_pair(id1, std::make_pair(id2, std::make_pair(PCA1, PCA2))) );
371 const Eigen::Vector3d &a2,
const Eigen::Vector3d &b2,
372 Eigen::Vector3d &PCA1, Eigen::Vector3d &PCA2)
382 auto bcrossb = b1.cross(b2);
383 auto mag_bcrossb = bcrossb.norm();
385 auto aminusa = a2-a1;
390 if( mag_bcrossb != 0)
391 dca = bcrossb.dot(aminusa) / mag_bcrossb;
411 double X = b1.dot(b2) - b1.dot(b1) * b2.dot(b2) / b2.dot(b1);
412 double Y = (a2.dot(b2) - a1.dot(b2)) - (a2.dot(b1) - a1.dot(b1)) * b2.dot(b2) / b2.dot(b1) ;
415 double F = b1.dot(b1) / b2.dot(b1);
416 double G = - (a2.dot(b1) - a1.dot(b1)) / b2.dot(b1);
417 double d = c * F + G;
430 std::vector<std::set<unsigned int>> connected_tracks;
431 std::set<unsigned int> connected;
432 std::set<unsigned int> used;
435 unsigned int id1 =
it.first;
436 unsigned int id2 =
it.second.first;
438 if( (used.find(id1) != used.end()) && (used.find(id2) != used.end()) )
440 if(
Verbosity() > 3) std::cout <<
" tracks " << id1 <<
" and " << id2 <<
" are both in used , skip them" << std::endl;
443 else if( (used.find(id1) == used.end()) && (used.find(id2) == used.end()))
445 if(
Verbosity() > 3) std::cout <<
" tracks " << id1 <<
" and " << id2 <<
" are both not in used , start a new connected set" << std::endl;
447 if(connected.size() > 0)
449 connected_tracks.push_back(connected);
451 if(
Verbosity() > 3) std::cout <<
" closing out set " << std::endl;
456 connected.insert(id1);
458 connected.insert(id2);
460 for(
auto cit : _track_pair_map)
462 unsigned int id3 = cit.first;
463 unsigned int id4 = cit.second.first;
464 if( (connected.find(id3) != connected.end()) || (connected.find(id4) != connected.end()) )
466 if(
Verbosity() > 3) std::cout <<
" found connection to " << id3 <<
" and " << id4 << std::endl;
467 connected.insert(id3);
469 connected.insert(id4);
476 if(connected.size() > 0)
478 connected_tracks.push_back(connected);
480 if(
Verbosity() > 3) std::cout <<
" closing out last set " << std::endl;
483 if(
Verbosity() > 3) std::cout <<
"connected_tracks size " << connected_tracks.size() << std::endl;
485 return connected_tracks;
494 unsigned int vtxid =
it;
495 if(
Verbosity() > 1) std::cout <<
"calculate average position for vertex " << vtxid << std::endl;
498 std::vector<double> vx;
499 std::vector<double> vy;
500 std::vector<double> vz;
502 double pca_median_x = 0.;
503 double pca_median_y = 0.;
504 double pca_median_z = 0.;
506 Eigen::Vector3d new_pca_avge(0.,0.,0.);
512 for (
auto cit=ret.first; cit!=ret.second; ++cit)
514 unsigned int tr1id = cit->second;
515 if(
Verbosity() > 2) std::cout <<
" vectors: get entries for track " << tr1id <<
" for vertex " << vtxid << std::endl;
519 for (
auto pit=pca_range.first; pit!=pca_range.second; ++pit)
521 unsigned int tr2id = pit->second.first;
523 Eigen::Vector3d PCA1 = pit->second.second.first;
524 Eigen::Vector3d PCA2 = pit->second.second.second;
527 std::cout <<
" vectors: tr1id " << tr1id <<
" tr2id " << tr2id
528 <<
" PCA1 " << PCA1.x() <<
" " << PCA1.y() <<
" " << PCA1.z()
529 <<
" PCA2 " << PCA2.x() <<
" " << PCA2.y() <<
" " << PCA2.z()
532 vx.push_back(PCA1.x());
533 vx.push_back(PCA2.x());
534 vy.push_back(PCA1.y());
535 vy.push_back(PCA2.y());
536 vz.push_back(PCA1.z());
537 vz.push_back(PCA2.z());
550 std::cout <<
" Vertex has only 2 tracks, use average for PCA: " << new_pca_avge.x() <<
" " << new_pca_avge.y() <<
" " << new_pca_avge.z() << std::endl;
559 if(
Verbosity() > 1) std::cout <<
"Median values: x " << pca_median_x <<
" y " << pca_median_y << std::endl;
562 for (
auto cit=ret.first; cit!=ret.second; ++cit)
564 unsigned int tr1id = cit->second;
565 if(
Verbosity() > 2) std::cout <<
" average: get entries for track " << tr1id <<
" for vertex " << vtxid << std::endl;
569 for (
auto pit=pca_range.first; pit!=pca_range.second; ++pit)
571 unsigned int tr2id = pit->second.first;
573 Eigen::Vector3d PCA1 = pit->second.second.first;
574 Eigen::Vector3d PCA2 = pit->second.second.second;
585 new_pca_avge += PCA1;
587 new_pca_avge += PCA2;
592 if(
Verbosity() > 1) std::cout <<
"Reject pair with tr1id " << tr1id <<
" tr2id " << tr2id << std::endl;
597 new_pca_avge = new_pca_avge / new_wt;
601 new_pca_avge.x() = pca_median_x;
602 new_pca_avge.y() = pca_median_y;
603 new_pca_avge.z() = pca_median_z;
617 if( (v.size() % 2) == 0)
621 auto m1 = v.begin() + v.size()/2;
622 std::nth_element(v.begin(), m1, v.end());
623 double median1 = v[v.size()/2];
625 auto m2 = v.begin() + v.size()/2 - 1;
626 std::nth_element(v.begin(),
m2, v.end());
627 double median2 = v[v.size()/2 - 1];
629 median = (median1 + median2) / 2.0;
630 if(
Verbosity() > 2) std::cout <<
"The vector size is " << v.size()
631 <<
" element m is " << v.size() / 2 <<
" = " << v[v.size()/2]
632 <<
" element m-1 is " << v.size() / 2 -1 <<
" = " << v[v.size()/2-1]
638 auto m = v.begin() + v.size()/2;
639 std::nth_element(v.begin(),
m, v.end());
640 median = v[v.size()/2];
641 if(
Verbosity() > 2) std::cout <<
"The vector size is " << v.size() <<
" element m is " << v.size() / 2 <<
" = " << v[v.size()/2] << std::endl;
659 std::cout <<
" average = " << avge << std::endl;