43 #include <phgenfit/Fitter.h>
44 #include <phgenfit/Measurement.h>
45 #include <phgenfit/PlanarMeasurement.h>
46 #include <phgenfit/SpacepointMeasurement.h>
47 #include <phgenfit/Track.h>
61 #include <phfield/PHFieldUtility.h>
62 #include <phgeom/PHGeomUtility.h>
64 #include <GenFit/AbsMeasurement.h>
65 #include <GenFit/EventDisplay.h>
66 #include <GenFit/Exception.h>
67 #include <GenFit/GFRaveConverters.h>
68 #include <GenFit/GFRaveTrackParameters.h>
69 #include <GenFit/GFRaveVertex.h>
70 #include <GenFit/GFRaveVertexFactory.h>
71 #include <GenFit/KalmanFitterInfo.h>
72 #include <GenFit/MeasuredStateOnPlane.h>
73 #include <GenFit/RKTrackRep.h>
75 #include <GenFit/TrackPoint.h>
78 #include <rave/ConstantMagneticField.h>
79 #include <rave/VacuumPropagator.h>
80 #include <rave/VertexFactory.h>
82 #include <TClonesArray.h>
83 #include <TRotation.h>
87 #include <TMatrixDSymfwd.h>
88 #include <TMatrixFfwd.h>
90 #include <TMatrixTSym.h>
91 #include <TMatrixTUtils.h>
92 #include <TVectorDfwd.h>
104 namespace genfit {
class AbsTrackRep; }
106 #define LogDebug(exp) std::cout << "DEBUG: " << __FILE__ << ": " << __LINE__ << ": " << exp << std::endl
107 #define LogError(exp) std::cout << "ERROR: " << __FILE__ << ": " << __LINE__ << ": " << exp << std::endl
108 #define LogWarning(exp) std::cout << "WARNING: " << __FILE__ << ": " << __LINE__ << ": " << exp << std::endl
112 #define _DEBUG_MODE_ 0
122 template<
class T >
T square(
T x ) {
return x*
x; }
125 SvtxTrackState_v1 create_track_state(
float pathlength,
const genfit::MeasuredStateOnPlane* gf_state )
129 out.
set_x(gf_state->getPos().x());
130 out.
set_y(gf_state->getPos().y());
131 out.
set_z(gf_state->getPos().z());
133 out.
set_px(gf_state->getMom().x());
134 out.
set_py(gf_state->getMom().y());
135 out.
set_pz(gf_state->getMom().z());
137 for (
int i = 0; i < 6; i++)
139 for (
int j = i; j < 6; j++)
140 { out.
set_error(i, j, gf_state->get6DCov()[i][j]); }
218 { std::cout <<
PHWHERE <<
" Layer " <<
layer <<
" is disabled." << std::endl; }
233 std::cout <<
PHWHERE <<
"Events processed: " <<
_event << std::endl;
242 vector<genfit::Track*> rf_gf_tracks;
243 vector<std::shared_ptr<PHGenFit::Track> > rf_phgf_tracks;
245 map<unsigned int, unsigned int> svtxtrack_genfittrack_map;
256 cout <<
" process SVTXTrack " << iter->first << endl;
266 std::shared_ptr<PHGenFit::Track> rf_phgf_track =
ReFitTrack(topNode, svtx_track);
269 svtxtrack_genfittrack_map[svtx_track->
get_id()] = rf_phgf_tracks.size();
270 rf_phgf_tracks.push_back(rf_phgf_track);
272 rf_gf_tracks.push_back(rf_phgf_track->getGenFitTrack());
273 if(
Verbosity() > 10) cout <<
"Done refitting input track" << svtx_track->
get_id() <<
" or rf_phgf_track " << rf_phgf_tracks.size() << endl;
277 cout <<
"failed refitting input track# " << iter->first << endl;
289 vector<genfit::Track*>
copy;
290 for (genfit::Track*
t : rf_gf_tracks)
292 copy.push_back(
new genfit::Track(*
t));
294 _fitter->getEventDisplay()->addEvent(copy);
298 std::vector<genfit::GFRaveVertex*> rave_vertices;
300 if (rf_gf_tracks.size() >= 2)
303 {cout <<
"Call Rave vertex finder" << endl;
304 cout <<
" rf_gf_tracks size " << rf_gf_tracks.size() << endl;
305 for(
long unsigned int i=0;i<rf_gf_tracks.size();++i)
307 cout <<
" track " << i <<
" num points " << rf_gf_tracks[i]->getNumPointsWithMeasurement() << endl;
317 std::cout <<
PHWHERE <<
"GFRaveVertexFactory::findVertices failed!";
321 if(
Verbosity() > 10 && rave_vertices.size() == 0)
323 cout <<
PHWHERE <<
" Rave found no vertices - SvtxVertexMapRefit will be empty" << endl;
337 std::shared_ptr<PHGenFit::Track> rf_phgf_track;
340 unsigned int itrack =0;
341 if (svtxtrack_genfittrack_map.find(iter->second->get_id()) != svtxtrack_genfittrack_map.end())
344 svtxtrack_genfittrack_map[iter->second->get_id()];
345 rf_phgf_track = rf_phgf_tracks[itrack];
352 unsigned int ivert = 0;
359 if(
Verbosity() > 20) cout <<
PHWHERE <<
" gf track " << itrack <<
" will add to track: _vertexmap_refit vertex " << ivert
360 <<
" with position x,y,z = " << vertex->
get_x() <<
" " << vertex->
get_y() <<
" " << vertex->
get_z() << endl;
362 std::shared_ptr<SvtxTrack> rf_track =
MakeSvtxTrack(iter->second, rf_phgf_track,
366 cout << __LINE__ << endl;
376 auto key = iter->first;
396 { iter->second->
CopyFrom( rf_track.get() ); }
402 auto key = iter->first;
413 cout << __LINE__ << endl;
419 rf_phgf_tracks.clear();
423 cout << __LINE__ << endl;
453 std::shared_ptr<PHGenFit::Track> rf_phgf_track =
ReFitTrack(topNode, svtx_track,
462 std::shared_ptr<SvtxTrack> rf_track =
MakeSvtxTrack(svtx_track,
463 rf_phgf_track, vertex);
478 LogError(
"No vertex in SvtxVertexMapRefit!");
482 cout << __LINE__ << endl;
484 for (genfit::GFRaveVertex* vertex : rave_vertices)
488 rave_vertices.clear();
495 cout << __LINE__ << endl;
547 {
new ((*_tca_trackmap)[i++])(
SvtxTrack_v2)( *pair.second ); }
561 {
new ((*_tca_trackmap_refit)[i++])(
SvtxTrack_v2)(*pair.second); }
568 {
new ((*_tca_primtrackmap)[i++])(
SvtxTrack_v2)(*pair.second); }
606 _eval_tree =
new TTree(
"T",
"PHGenFitTrkFitter Evaluation");
658 "PHCompositeNode",
"DST"));
661 cerr <<
PHWHERE <<
"DST Node missing, doing nothing." << endl;
668 "PHCompositeNode",
"SVTX"));
674 cout <<
"SVTX node added" << endl;
684 cout <<
"Svtx/SvtxTrackMapRefit node added" << endl;
693 tb_node->
addNode(primary_tracks_node);
695 cout <<
"Svtx/PrimaryTrackMap node added" << endl;
702 tb_node->
addNode(vertexes_node);
704 cout <<
"Svtx/SvtxVertexMapRefit node added" << endl;
736 _truth_container = findNode::getClass<PHG4TruthInfoContainer>(topNode,
"G4TruthInfo");
740 _clustermap = findNode::getClass<TrkrClusterContainer>(topNode,
"TRKR_CLUSTER");
741 if (!_clustermap &&
_event < 2)
743 cout <<
PHWHERE <<
" TRKR_CLUSTER node not found on node tree"
749 _trackmap = findNode::getClass<SvtxTrackMap>(topNode,
"SvtxTrackMap");
752 cout <<
PHWHERE <<
" SvtxTrackMap node not found on node tree"
758 _vertexmap = findNode::getClass<SvtxVertexMap>(topNode,
"SvtxVertexMap");
761 cout <<
PHWHERE <<
" SvtxVertexrMap node not found on node tree"
770 "SvtxTrackMapRefit");
773 cout <<
PHWHERE <<
" SvtxTrackMapRefit node not found on node tree"
786 cout <<
PHWHERE <<
" PrimaryTrackMap node not found on node tree"
794 "SvtxVertexMapRefit");
797 cout <<
PHWHERE <<
" SvtxVertexMapRefit node not found on node tree"
824 cerr <<
PHWHERE <<
" Input SvtxTrack is nullptr!" << endl;
828 auto geom_container_intt = findNode::getClass<PHG4CylinderGeomContainer>(topNode,
"CYLINDERGEOM_INTT");
829 assert( geom_container_intt );
831 auto geom_container_mvtx = findNode::getClass<PHG4CylinderGeomContainer>(topNode,
"CYLINDERGEOM_MVTX");
832 assert( geom_container_mvtx );
835 auto geom_container_micromegas = findNode::getClass<PHG4CylinderGeomContainer>(topNode,
"CYLINDERGEOM_MICROMEGAS_FULL");
838 TVector3 seed_mom(100, 0, 0);
839 TVector3 seed_pos(0, 0, 0);
840 TMatrixDSym seed_cov(6);
841 for (
int i = 0; i < 6; i++)
843 for (
int j = 0; j < 6; j++)
845 seed_cov[i][j] = 100.;
850 std::vector<PHGenFit::Measurement*> measurements;
873 #if _DEBUG_MODE_ == 1
886 double x = rand.Gaus(0, dxy);
887 double y = rand.Gaus(0, dxy);
888 double z = rand.Gaus(0, dz);
891 for (
int i = 0; i < 3; i++)
892 for (
int j = 0; j < 3; j++)
895 cov[0][0] = dxy * dxy;
896 cov[1][1] = dxy * dxy;
901 measurements.push_back(meas);
905 const double vertex_chi2_over_dnf_cut = 1000;
906 const double vertex_cov_element_cut = 10000;
913 bool is_vertex_cov_sane =
true;
914 for (
unsigned int i = 0; i < 3; i++)
915 for (
unsigned int j = 0; j < 3; j++)
921 if (!(invertex->
get_error(i, j) > 0 and invertex->
get_error(i, j) < vertex_cov_element_cut))
922 is_vertex_cov_sane =
false;
926 if (is_vertex_cov_sane)
930 measurements.push_back(meas);
941 std::map<float, TrkrDefs::cluskey> m_r_cluster_id;
949 m_r_cluster_id.insert(std::pair<float, TrkrDefs::cluskey>(r, cluster_key));
951 if(
Verbosity() > 10) cout <<
" Layer " << layer_out <<
" cluster " << cluster_key <<
" radius " << r << endl;
954 for (
auto iter = m_r_cluster_id.begin();
955 iter != m_r_cluster_id.end();
976 <<
": ID: " << cluster_key
977 <<
": layer: " << output_layer
983 seed_mom.SetPhi(
pos.Phi());
984 seed_mom.SetTheta(
pos.Theta());
998 double ladder_location[3] = {0.0, 0.0, 0.0};
999 auto geom =
static_cast<CylinderGeom_Mvtx*
>(geom_container_mvtx->GetLayerGeom(layer));
1002 0, chip_index, ladder_location);
1006 n.SetXYZ(ladder_location[0], ladder_location[1], 0);
1007 n.RotateZ(geom->get_stave_phi_tilt());
1013 auto geom =
static_cast<CylinderGeomIntt*
>(geom_container_intt->GetLayerGeom(layer));
1014 double hit_location[3] = {0.0, 0.0, 0.0};
1020 n.SetXYZ(hit_location[0], hit_location[1], 0);
1021 n.RotateZ(geom->get_strip_phi_tilt());
1029 assert( geom_container_micromegas );
1035 n = geom->get_world_from_local_vect( tileid, TVector3( 0, 1, 0 ) );
1046 cout <<
"Add meas layer " << layer <<
" cluskey " << cluster_key
1048 <<
" pos.X " <<
pos.X() <<
" pos.Y " <<
pos.Y() <<
" pos.Z " <<
pos.Z()
1049 <<
" n.X " <<
n.X() <<
" n.Y " <<
n.Y()
1054 measurements.push_back(meas);
1073 track->addMeasurements(measurements);
1080 if (
_fitter->processTrack(track.get(),
false) != 0)
1098 cout <<
" track->getChisq() " << track->get_chi2() <<
" get_ndf " << track->get_ndf()
1099 <<
" mom.X " << track->get_mom().X()
1100 <<
" mom.Y " << track->get_mom().Y()
1101 <<
" mom.Z " << track->get_mom().Z()
1112 const std::shared_ptr<PHGenFit::Track>& phgf_track,
const SvtxVertex* vertex)
1114 double chi2 = phgf_track->get_chi2();
1115 double ndf = phgf_track->get_ndf();
1117 TVector3 vertex_position(0, 0, 0);
1118 TMatrixF vertex_cov(3, 3);
1127 vertex_position.SetXYZ(first_point->
get_x(), first_point->
get_y(), first_point->
get_z());
1130 std::cout <<
PHWHERE <<
"Using: truth vertex: {" << vertex_position.X() <<
", " << vertex_position.Y() <<
", " << vertex_position.Z() <<
"} " << std::endl;
1133 std::cout <<
PHWHERE <<
"PHG4TruthInfoContainer not found. Cannot use truth vertex." << std::endl;
1138 vertex_position.SetXYZ(vertex->
get_x(), vertex->
get_y(),
1143 for (
int i = 0; i < 3; i++)
1144 for (
int j = 0; j < 3; j++)
1145 vertex_cov[i][j] = vertex->
get_error(i, j);
1148 std::unique_ptr<genfit::MeasuredStateOnPlane> gf_state_beam_line_ca;
1151 gf_state_beam_line_ca.reset(phgf_track->extrapolateToLine(vertex_position,
1152 TVector3(0., 0., 1.)));
1159 if (!gf_state_beam_line_ca)
return nullptr;
1168 double u = gf_state_beam_line_ca->getState()[3];
1169 double v = gf_state_beam_line_ca->getState()[4];
1171 double du2 = gf_state_beam_line_ca->getCov()[3][3];
1172 double dv2 = gf_state_beam_line_ca->getCov()[4][4];
1177 auto out_track = std::make_shared<SvtxTrack_v2>(*svtx_track);
1180 out_track->clear_states();
1188 out_track->insert_state( &first );
1191 out_track->set_dca2d(u);
1192 out_track->set_dca2d_error(sqrt(du2 + dvr2));
1194 std::unique_ptr<genfit::MeasuredStateOnPlane> gf_state_vertex_ca;
1197 gf_state_vertex_ca.reset( phgf_track->extrapolateToPoint(vertex_position) );
1204 if (!gf_state_vertex_ca)
1210 TVector3
mom = gf_state_vertex_ca->getMom();
1211 TVector3
pos = gf_state_vertex_ca->getPos();
1212 TMatrixDSym cov = gf_state_vertex_ca->get6DCov();
1218 u = gf_state_vertex_ca->getState()[3];
1219 v = gf_state_vertex_ca->getState()[4];
1221 du2 = gf_state_vertex_ca->getCov()[3][3];
1222 dv2 = gf_state_vertex_ca->getCov()[4][4];
1224 double dca3d = sqrt(u * u + v * v);
1225 double dca3d_error = sqrt(du2 + dv2 + dvr2 + dvz2);
1227 out_track->set_dca(dca3d);
1228 out_track->set_dca_error(dca3d_error);
1299 float dca3d_xy = NAN;
1300 float dca3d_z = NAN;
1301 float dca3d_xy_error = NAN;
1302 float dca3d_z_error = NAN;
1306 TMatrixF pos_in(3, 1);
1307 TMatrixF cov_in(3, 3);
1308 TMatrixF pos_out(3, 1);
1309 TMatrixF cov_out(3, 3);
1312 TMatrixDSym cov6(6, 6);
1314 gf_state_vertex_ca->get6DStateCov(state6, cov6);
1316 TVector3 vn(state6[3], state6[4], state6[5]);
1319 pos_in[0][0] = state6[0] - vertex_position.X();
1320 pos_in[1][0] = state6[1] - vertex_position.Y();
1321 pos_in[2][0] = state6[2] - vertex_position.Z();
1323 for (
int i = 0; i < 3; ++i)
1325 for (
int j = 0; j < 3; ++j)
1327 cov_in[i][j] = cov6[i][j] + vertex_cov[i][j];
1336 cout <<
" vn.X " << vn.X() <<
" vn.Y " << vn.Y() <<
" vn.Z " << vn.Z() << endl;
1337 cout <<
" pos_in.X " << pos_in[0][0] <<
" pos_in.Y " << pos_in[1][0] <<
" pos_in.Z " << pos_in[2][0] << endl;
1338 cout <<
" pos_out.X " << pos_out[0][0] <<
" pos_out.Y " << pos_out[1][0] <<
" pos_out.Z " << pos_out[2][0] << endl;
1342 dca3d_xy = pos_out[0][0];
1343 dca3d_z = pos_out[2][0];
1344 dca3d_xy_error = sqrt(cov_out[0][0]);
1345 dca3d_z_error = sqrt(cov_out[2][2]);
1348 cout << __LINE__ <<
": Vertex: ----------------" << endl;
1349 vertex_position.Print();
1352 cout << __LINE__ <<
": State: ----------------" << endl;
1356 cout << __LINE__ <<
": Mean: ----------------" << endl;
1358 cout <<
"===>" << endl;
1361 cout << __LINE__ <<
": Cov: ----------------" << endl;
1363 cout <<
"===>" << endl;
1375 out_track->set_dca3d_xy(dca3d_xy);
1376 out_track->set_dca3d_z(dca3d_z);
1377 out_track->set_dca3d_xy_error(dca3d_xy_error);
1378 out_track->set_dca3d_z_error(dca3d_z_error);
1382 out_track->set_chisq(chi2);
1383 out_track->set_ndf(ndf);
1384 out_track->set_charge(phgf_track->get_charge());
1386 out_track->set_px(mom.Px());
1387 out_track->set_py(mom.Py());
1388 out_track->set_pz(mom.Pz());
1390 out_track->set_x(pos.X());
1391 out_track->set_y(pos.Y());
1392 out_track->set_z(pos.Z());
1394 for (
int i = 0; i < 6; i++)
1396 for (
int j = i; j < 6; j++)
1398 out_track->set_error(i, j, cov[i][j]);
1463 cout << __LINE__ << endl;
1466 const genfit::Track* gftrack = phgf_track->getGenFitTrack();
1467 const genfit::AbsTrackRep* rep = gftrack->getCardinalRep();
1468 for (
unsigned int id = 0;
id < gftrack->getNumPointsWithMeasurement(); ++id)
1470 genfit::TrackPoint* trpoint = gftrack->getPointWithMeasurementAndFitterInfo(
id, gftrack->getCardinalRep());
1479 genfit::KalmanFitterInfo* kfi =
static_cast<genfit::KalmanFitterInfo*
>(trpoint->getFitterInfo(rep));
1487 const genfit::MeasuredStateOnPlane* gf_state =
nullptr;
1491 gf_state = &kfi->getFittedState(
true);
1504 genfit::MeasuredStateOnPlane temp;
1505 float pathlength = -phgf_track->extrapolateToPoint(temp, vertex_position,
id);
1508 auto state = create_track_state( pathlength, gf_state );
1509 out_track->insert_state( &state );
1515 <<
": " << pathlength <<
" => "
1516 << sqrt(state->get_x() * state->get_x() + state->get_y() * state->get_y())
1525 unsigned int id_min = 0;
1526 for (
auto iter = svtx_track->begin_cluster_keys(); iter != svtx_track->end_cluster_keys(); ++iter)
1529 auto cluster_key = *iter;
1538 TVector3
pos(cluster->getPosition(0), cluster->getPosition(1), cluster->getPosition(2));
1539 float r_cluster = std::sqrt(
square(pos[0]) +
square(pos[1]) );
1543 unsigned int id = id_min;
1544 for( ;
id < gftrack->getNumPointsWithMeasurement(); ++id)
1547 auto trpoint = gftrack->getPointWithMeasurementAndFitterInfo(
id, rep);
1548 if (!trpoint)
continue;
1550 auto kfi =
static_cast<genfit::KalmanFitterInfo*
>(trpoint->getFitterInfo(rep));
1553 const genfit::MeasuredStateOnPlane* gf_state =
nullptr;
1557 gf_state = &kfi->getFittedState(
true);
1562 {
LogWarning(
"Failed to get kf fitted state"); }
1566 if( !gf_state )
continue;
1568 float r_track = std::sqrt(
square( gf_state->getPos().x() ) +
square( gf_state->getPos().y() ) );
1569 if( r_track > r_cluster )
break;
1574 genfit::MeasuredStateOnPlane gf_state;
1575 float pathlength = 0;
1578 if(
id > 0 ) id_min =
id-1;
1583 auto trpoint = gftrack->getPointWithMeasurementAndFitterInfo(id_min, rep);
1584 if( !trpoint )
continue;
1586 auto kfi =
static_cast<genfit::KalmanFitterInfo*
>(trpoint->getFitterInfo(rep));
1587 gf_state = *kfi->getForwardUpdate();
1588 pathlength = gf_state.extrapolateToPoint( pos );
1589 auto tmp = *kfi->getBackwardUpdate();
1590 pathlength -=
tmp.extrapolateToPoint( vertex_position );
1593 { std::cerr <<
PHWHERE <<
"Failed to forward extrapolate from id " << id_min <<
" to disabled layer " << layer << std::endl; }
1599 if(
id > 0 && id < gftrack->getNumPointsWithMeasurement() )
1602 auto trpoint = gftrack->getPointWithMeasurementAndFitterInfo(
id, rep);
1603 if( !trpoint )
continue;
1605 auto kfi =
static_cast<genfit::KalmanFitterInfo*
>(trpoint->getFitterInfo(rep));
1606 genfit::KalmanFittedStateOnPlane gf_state_backward = *kfi->getBackwardUpdate();
1607 gf_state_backward.extrapolateToPlane( gf_state.getPlane() );
1608 gf_state = genfit::calcAverageState( gf_state, gf_state_backward );
1611 { std::cerr <<
PHWHERE <<
"Failed to backward extrapolate from id " <<
id <<
" to disabled layer " << layer << std::endl; }
1616 auto state = create_track_state( pathlength, &gf_state );
1617 out_track->insert_state( &state );
1630 const std::vector<genfit::GFRaveVertex*>& rave_vertices,
1631 const std::vector<genfit::Track*>& gf_tracks)
1634 if(
Verbosity() > 0) cout <<
"Rave vertices size " << rave_vertices.size() << endl;
1635 if(rave_vertices.size() > 0)
1637 for (
unsigned int ivtx = 0; ivtx < rave_vertices.size(); ++ivtx)
1639 genfit::GFRaveVertex* rave_vtx = rave_vertices[ivtx];
1647 if(
Verbosity() > 0) cout <<
" ivtx " << ivtx <<
" has Z = " << rave_vtx->getPos().Z() << endl;
1650 svtx_vtx.
set_chisq(rave_vtx->getChi2());
1651 svtx_vtx.
set_ndof(rave_vtx->getNdf());
1656 for (
int i = 0; i < 3; i++)
1657 for (
int j = 0; j < 3; j++)
1658 svtx_vtx.
set_error(i, j, rave_vtx->getCov()[i][j]);
1660 for (
unsigned int i = 0; i < rave_vtx->getNTracks(); i++)
1663 const genfit::Track* rave_track =
1664 rave_vtx->getParameters(i)->getTrack();
1665 for (
unsigned int j = 0; j < gf_tracks.size(); j++)
1667 if (rave_track == gf_tracks[j])
1671 if(
Verbosity() > 0) cout <<
" rave vertex " << ivtx <<
" at Z " << svtx_vtx.
get_position(2) <<
" rave track " << i <<
" genfit track ID " << j << endl;
1678 if(
Verbosity() > 0) cout <<
"insert svtx_vtx into _vertexmap_refit " << endl;
1785 const TVector3&
n,
const TMatrixF& pos_in,
const TMatrixF& cov_in,
1786 TMatrixF& pos_out, TMatrixF& cov_out)
const
1788 if (pos_in.GetNcols() != 1 || pos_in.GetNrows() != 3)
1794 if (cov_in.GetNcols() != 3 || cov_in.GetNrows() != 3)
1800 TVector3 Z_uvn(u.Z(), v.Z(), n.Z());
1801 TVector3 up_uvn = TVector3(0., 0., 1.).Cross(Z_uvn);
1803 if (up_uvn.Mag() < 0.00001)
1816 float phi = -TMath::ATan2(up_uvn.Y(), up_uvn.X());
1818 R[0][1] = -sin(phi);
1837 pos_out.ResizeTo(3, 1);
1838 cov_out.ResizeTo(3, 3);
1840 pos_out = R * pos_in;
1841 cov_out = R * cov_in * R_T;
1847 const TVector3&
v,
const TVector3&
n,
const TMatrixF& cov_in,
1848 TMatrixF& cov_out)
const
1861 if (!(
abs(R.Determinant() - 1) < 0.01))
1864 LogWarning(
"!(abs(R.Determinant()-1)<0.0001)");
1868 if (R.GetNcols() != 3 || R.GetNrows() != 3)
1871 LogWarning(
"R.GetNcols() != 3 || R.GetNrows() != 3");
1875 if (cov_in.GetNcols() != 3 || cov_in.GetNrows() != 3)
1878 LogWarning(
"cov_in.GetNcols() != 3 || cov_in.GetNrows() != 3");
1886 cov_out.ResizeTo(3, 3);
1888 cov_out = R * cov_in * R_T;
1894 const TVector3&
n,
const TMatrixF& pos_in,
const TMatrixF& cov_in,
1895 TMatrixF& pos_out, TMatrixF& cov_out)
const
1897 if (pos_in.GetNcols() != 1 || pos_in.GetNrows() != 3)
1903 if (cov_in.GetNcols() != 3 || cov_in.GetNrows() != 3)
1911 TVector3
r = n.Cross(TVector3(0., 0., 1.));
1912 if (r.Mag() < 0.00001)
1925 float phi = -TMath::ATan2(r.Y(), r.X());
1927 R[0][1] = -sin(phi);
1946 pos_out.ResizeTo(3, 1);
1947 cov_out.ResizeTo(3, 3);
1949 pos_out = R * pos_in;
1950 cov_out = R * cov_in * R_T;
1960 const TVector3
y,
const TVector3
z,
const TVector3 xp,
const TVector3 yp,
1961 const TVector3 zp)
const
1965 TVector3 xu = x.Unit();
1966 TVector3 yu = y.Unit();
1967 TVector3 zu = z.Unit();
1969 const float max_diff = 0.01;
1972 abs(xu * yu) < max_diff and
1973 abs(xu * zu) < max_diff and
1974 abs(yu * zu) < max_diff))
1981 TVector3 xpu = xp.Unit();
1982 TVector3 ypu = yp.Unit();
1983 TVector3 zpu = zp.Unit();
1986 abs(xpu * ypu) < max_diff and
1987 abs(xpu * zpu) < max_diff and
1988 abs(ypu * zpu) < max_diff))
2000 TVector3
u(xpu.Dot(xu), xpu.Dot(yu), xpu.Dot(zu));
2001 TVector3
v(ypu.Dot(xu), ypu.Dot(yu), ypu.Dot(zu));
2002 TVector3
n(zpu.Dot(xu), zpu.Dot(yu), zpu.Dot(zu));
2006 std::shared_ptr<TRotation> rotation(
new TRotation());
2010 rotation->RotateAxes(
u,
v,
n);
2012 R[0][0] = rotation->XX();
2013 R[0][1] = rotation->XY();
2014 R[0][2] = rotation->XZ();
2015 R[1][0] = rotation->YX();
2016 R[1][1] = rotation->YY();
2017 R[1][2] = rotation->YZ();
2018 R[2][0] = rotation->ZX();
2019 R[2][1] = rotation->ZY();
2020 R[2][2] = rotation->ZZ();