packages/trackbase_historic/ActsTransformations.cc

0001 #include "ActsTransformations.h"
0002 #include "SvtxTrack.h"
0003 #include "SvtxTrackState.h"
0004 #include "SvtxTrackState_v3.h"
0005
0006 #include <trackbase/ActsSourceLink.h>
0007 #include <trackbase/InttDefs.h>
0008 #include <trackbase/MvtxDefs.h>
0009 #include <trackbase/TrkrCluster.h>
0010
0011 #include <Acts/EventData/ParticleHypothesis.hpp>
0012 #include <cmath>
0013
0014 namespace
0015 {
0016   /// square
0017   template <class T>
0018   inline constexpr T square(const T& x)
0019   {
0020     return x * x;
0021   }
0022
0023   /// get radius from coordinates
0024   template <class T>
0025   T radius(const T& x, const T& y)
0026   {
0027     return std::sqrt(square(x) + square(y));
0028   }
0029
0030   /// templatize print matrix method
0031   template <class T>
0032   void print_matrix(const std::string& message, const T& matrix)
0033   {
0034     std::cout << std::endl
0035               << message << std::endl;
0036     for (int i = 0; i < matrix.rows(); ++i)
0037     {
0038       for (int j = 0; j < matrix.cols(); ++j)
0039       {
0040         std::cout << matrix(i, j) << ", ";
0041       }
0042
0043       std::cout << std::endl;
0044     }
0045   }
0046
0047 }  // namespace
0048
0049 //_______________________________________________________________________________
0050 Acts::BoundSquareMatrix ActsTransformations::rotateSvtxTrackCovToActs(const SvtxTrack* track) const
0051 {
0052   return rotateSvtxTrackCovToActs(track->find_state(0.0)->second);
0053 }
0054
0055 //_______________________________________________________________________________
0056 Acts::BoundSquareMatrix ActsTransformations::rotateSvtxTrackCovToActs(
0057     const SvtxTrackState* state) const
0058 {
0059   Acts::BoundSquareMatrix svtxCovariance = Acts::BoundSquareMatrix::Zero();
0060
0061   for (int i = 0; i < 6; ++i)
0062   {
0063     for (int j = 0; j < 6; ++j)
0064     {
0065       svtxCovariance(i, j) = state->get_error(i, j);
0066       /// Convert Svtx to mm and GeV units as Acts expects
0067       if (i < 3 && j < 3)
0068       {
0069         svtxCovariance(i, j) *= Acts::UnitConstants::cm2;
0070       }
0071       else if (i < 3)
0072       {
0073         svtxCovariance(i, j) *= Acts::UnitConstants::cm;
0074       }
0075       else if (j < 3)
0076       {
0077         svtxCovariance(i, j) *= Acts::UnitConstants::cm;
0078       }
0079     }
0080   }
0081
0082   printMatrix("svtx covariance, acts units: ", svtxCovariance);
0083
0084   const double px = state->get_px();
0085   const double py = state->get_py();
0086   const double pz = state->get_pz();
0087   const double p2 = square(px) + square(py) + square(pz);
0088   const double p = std::sqrt(p2);
0089   const double invp = 1. / p;
0090
0091   const double uPx = px / p;
0092   const double uPy = py / p;
0093   const double uPz = pz / p;
0094
0095   // Acts version
0096   const double cosTheta = uPz;
0097   const double sinTheta = std::sqrt(square(uPx) + square(uPy));
0098   const double invSinTheta = 1. / sinTheta;
0099   const double cosPhi = uPx * invSinTheta;  // equivalent to x/r
0100   const double sinPhi = uPy * invSinTheta;  // equivalent to y/r
0101
0102   /// First we rotate to (x,y,z,time,Tx,Ty,Tz,q/p) to take advantage of the
0103   /// already created Acts rotation matrix from this basis into the Acts local basis
0104   /// We basically go backwards from rotateActsCovToSvtxTrack to get the Acts cov
0105   /// from the SvtxTrack cov
0106
0107   // rotate to (x,y,z,t,upx,upy,upz,q/p)
0108   Acts::ActsMatrix<8, 6> sphenixRot = Acts::ActsMatrix<8, 6>::Zero();
0109   sphenixRot(0, 0) = 1;
0110   sphenixRot(1, 1) = 1;
0111   sphenixRot(2, 2) = 1;
0112   sphenixRot(4, 3) = invp;
0113   sphenixRot(5, 4) = invp;
0114   sphenixRot(6, 5) = invp;
0115   sphenixRot(7, 3) = uPx / p2;
0116   sphenixRot(7, 4) = uPy / p2;
0117   sphenixRot(7, 5) = uPz / p2;
0118
0119   const auto rotatedMatrix = sphenixRot * svtxCovariance * sphenixRot.transpose();
0120
0121   // global to local transformation
0122   /* from https://github.com/acts-project/acts/blob/394cfdb308956de93f90ab3162040bb6d835027d/Core/src/Propagator/detail/CovarianceEngine.cpp#L31 */
0123   Acts::FreeToBoundMatrix jacobianGlobalToLocal = Acts::FreeToBoundMatrix::Zero();
0124   jacobianGlobalToLocal(Acts::eBoundLoc0, 0) = -sinPhi;
0125   jacobianGlobalToLocal(Acts::eBoundLoc0, 1) = cosPhi;
0126   jacobianGlobalToLocal(Acts::eBoundLoc1, 0) = -cosPhi * cosTheta;
0127   jacobianGlobalToLocal(Acts::eBoundLoc1, 1) = -sinPhi * cosTheta;
0128   jacobianGlobalToLocal(Acts::eBoundLoc1, 2) = sinTheta;
0129   jacobianGlobalToLocal(Acts::eBoundTime, 3) = 1.;
0130   jacobianGlobalToLocal(Acts::eBoundPhi, 4) = -sinPhi * invSinTheta;
0131   jacobianGlobalToLocal(Acts::eBoundPhi, 5) = cosPhi * invSinTheta;
0132   jacobianGlobalToLocal(Acts::eBoundTheta, 4) = cosPhi * cosTheta;
0133   jacobianGlobalToLocal(Acts::eBoundTheta, 5) = sinPhi * cosTheta;
0134   jacobianGlobalToLocal(Acts::eBoundTheta, 6) = -sinTheta;
0135   jacobianGlobalToLocal(Acts::eBoundQOverP, 7) = 1.;
0136
0137   Acts::BoundSquareMatrix actsLocalCov = jacobianGlobalToLocal * rotatedMatrix * jacobianGlobalToLocal.transpose();
0138
0139   /*
0140    * need to assign the covariance matrix diagonal element corresponding to the time coordinate manually,
0141    * since it is lost in the conversion to Svtx coordinates
0142    */
0143   actsLocalCov(Acts::eBoundTime, Acts::eBoundTime) = 1.;
0144
0145   printMatrix("Rotated to Acts local cov : ", actsLocalCov);
0146   return actsLocalCov;
0147 }
0148
0149 //_______________________________________________________________________________
0150 Acts::BoundSquareMatrix ActsTransformations::rotateActsCovToSvtxTrack(const ActsTrackFittingAlgorithm::TrackParameters& params) const
0151 {
0152   const auto covarianceMatrix = *params.covariance();
0153   printMatrix("Initial Acts covariance: ", covarianceMatrix);
0154
0155   const double px = params.momentum().x();
0156   const double py = params.momentum().y();
0157   const double pz = params.momentum().z();
0158   const double p = params.momentum().norm();
0159   const double p2 = square(p);
0160
0161   const double uPx = px / p;
0162   const double uPy = py / p;
0163   const double uPz = pz / p;
0164
0165   const double cosTheta = uPz;
0166   const double sinTheta = std::sqrt(square(uPx) + square(uPy));
0167   const double invSinTheta = 1. / sinTheta;
0168   const double cosPhi = uPx * invSinTheta;
0169   const double sinPhi = uPy * invSinTheta;
0170
0171   // local to global transformation
0172   /* from https://github.com/acts-project/acts/blob/394cfdb308956de93f90ab3162040bb6d835027d/Core/src/Propagator/detail/CovarianceEngine.cpp#L222 */
0173   Acts::BoundToFreeMatrix jacobianLocalToGlobal = Acts::BoundToFreeMatrix::Zero();
0174   jacobianLocalToGlobal(0, Acts::eBoundLoc0) = -sinPhi;
0175   jacobianLocalToGlobal(0, Acts::eBoundLoc1) = -cosPhi * cosTheta;
0176   jacobianLocalToGlobal(1, Acts::eBoundLoc0) = cosPhi;
0177   jacobianLocalToGlobal(1, Acts::eBoundLoc1) = -sinPhi * cosTheta;
0178   jacobianLocalToGlobal(2, Acts::eBoundLoc1) = sinTheta;
0179   jacobianLocalToGlobal(3, Acts::eBoundTime) = 1;
0180   jacobianLocalToGlobal(4, Acts::eBoundPhi) = -sinTheta * sinPhi;
0181   jacobianLocalToGlobal(4, Acts::eBoundTheta) = cosTheta * cosPhi;
0182   jacobianLocalToGlobal(5, Acts::eBoundPhi) = sinTheta * cosPhi;
0183   jacobianLocalToGlobal(5, Acts::eBoundTheta) = cosTheta * sinPhi;
0184   jacobianLocalToGlobal(6, Acts::eBoundTheta) = -sinTheta;
0185   jacobianLocalToGlobal(7, Acts::eBoundQOverP) = 1;
0186
0187   // Covariance is now an 8x8 matrix in basis (x,y,z,time,Tx,Ty,Tz,q/p)
0188   const auto rotatedMatrix = jacobianLocalToGlobal * covarianceMatrix * jacobianLocalToGlobal.transpose();
0189
0190   // Now rotate to x,y,z, px,py,pz
0191   Acts::ActsMatrix<6, 8> sphenixRot = Acts::ActsMatrix<6, 8>::Zero();
0192   sphenixRot(0, 0) = 1;
0193   sphenixRot(1, 1) = 1;
0194   sphenixRot(2, 2) = 1;
0195   sphenixRot(3, 4) = p;
0196   sphenixRot(4, 5) = p;
0197   sphenixRot(5, 6) = p;
0198   sphenixRot(3, 7) = uPx * p2;
0199   sphenixRot(4, 7) = uPy * p2;
0200   sphenixRot(5, 7) = uPz * p2;
0201
0202   Acts::BoundSquareMatrix globalCov = sphenixRot * rotatedMatrix * sphenixRot.transpose();
0203   printMatrix("Global sPHENIX cov : ", globalCov);
0204
0205   /// Convert to sPHENIX units
0206   for (int i = 0; i < 6; ++i)
0207   {
0208     for (int j = 0; j < 6; ++j)
0209     {
0210       if (i < 3 && j < 3)
0211       {
0212         globalCov(i, j) /= Acts::UnitConstants::cm2;
0213       }
0214       else if (i < 3)
0215       {
0216         globalCov(i, j) /= Acts::UnitConstants::cm;
0217       }
0218       else if (j < 3)
0219       {
0220         globalCov(i, j) /= Acts::UnitConstants::cm;
0221       }
0222     }
0223   }
0224
0225   printMatrix("Global sphenix cov after unit conv: ", globalCov);
0226
0227   return globalCov;
0228 }
0229
0230 void ActsTransformations::printMatrix(const std::string& message, const Acts::BoundSquareMatrix& matrix) const
0231 {
0232   if (m_verbosity > 10)
0233   {
0234     print_matrix(message, matrix);
0235   }
0236 }
0237
0238 void ActsTransformations::calculateDCA(const Acts::BoundTrackParameters& param,
0239                                        const Acts::Vector3& vertex,
0240                                        Acts::BoundSquareMatrix cov,
0241                                        Acts::GeometryContext& geoCtxt,
0242                                        float& dca3Dxy,
0243                                        float& dca3Dz,
0244                                        float& dca3DxyCov,
0245                                        float& dca3DzCov) const
0246 {
0247   Acts::Vector3 pos = param.position(geoCtxt);
0248   Acts::Vector3 mom = param.momentum();
0249
0250   /// Correct for initial vertex estimation
0251   pos -= vertex;
0252
0253   Acts::ActsSquareMatrix<3> posCov;
0254   for (int i = 0; i < 3; ++i)
0255   {
0256     for (int j = 0; j < 3; ++j)
0257     {
0258       posCov(i, j) = cov(i, j);
0259     }
0260   }
0261
0262   Acts::Vector3 r = mom.cross(Acts::Vector3(0., 0., 1.));
0263   float phi = atan2(r(1), r(0));
0264
0265   Acts::RotationMatrix3 rot;
0266   Acts::RotationMatrix3 rot_T;
0267   rot(0, 0) = std::cos(phi);
0268   rot(0, 1) = -std::sin(phi);
0269   rot(0, 2) = 0;
0270   rot(1, 0) = std::sin(phi);
0271   rot(1, 1) = std::cos(phi);
0272   rot(1, 2) = 0;
0273   rot(2, 0) = 0;
0274   rot(2, 1) = 0;
0275   rot(2, 2) = 1;
0276
0277   rot_T = rot.transpose();
0278
0279   Acts::Vector3 pos_R = rot * pos;
0280   Acts::ActsSquareMatrix<3> rotCov = rot * posCov * rot_T;
0281
0282   dca3Dxy = pos_R(0);
0283   dca3Dz = pos_R(2);
0284   dca3DxyCov = rotCov(0, 0);
0285   dca3DzCov = rotCov(2, 2);
0286 }
0287
0288 void ActsTransformations::fillSvtxTrackStates(const Acts::ConstVectorMultiTrajectory& traj,
0289                                               const size_t& trackTip,
0290                                               SvtxTrack* svtxTrack,
0291                                               Acts::GeometryContext& geoContext) const
0292 {
0293   traj.visitBackwards(trackTip, [&](const auto& state)
0294                       {
0295
0296       /// Only fill the track states with non-outlier measurement
0297       const auto typeFlags = state.typeFlags();
0298       if( !typeFlags.test(Acts::TrackStateFlag::MeasurementFlag) )
0299       { return true; }
0300
0301       // only fill for state vectors with proper smoothed parameters
0302       if( !state.hasSmoothed()) { return true;
0303 }
0304
0305       // create svtx state vector with relevant pathlength
0306       const float pathlength = state.pathLength() / Acts::UnitConstants::cm;
0307       SvtxTrackState_v3 out( pathlength );
0308
0309       // get smoothed fitted parameters
0310       const Acts::BoundTrackParameters params(
0311         state.referenceSurface().getSharedPtr(),
0312         state.smoothed(),
0313         state.smoothedCovariance(),
0314         Acts::ParticleHypothesis::pion());
0315
0316       // local position
0317       const auto localX = params.parameters()[Acts::eBoundLoc0];
0318       const auto localY = params.parameters()[Acts::eBoundLoc1];
0319       out.set_localX(localX / Acts::UnitConstants::cm);
0320       out.set_localY(localY / Acts::UnitConstants::cm);
0321
0322       // global position
0323       const auto global = params.position(geoContext);
0324       out.set_x(global.x() / Acts::UnitConstants::cm);
0325       out.set_y(global.y() / Acts::UnitConstants::cm);
0326       out.set_z(global.z() / Acts::UnitConstants::cm);
0327
0328       // momentum
0329       const auto momentum = params.momentum();
0330       out.set_px(momentum.x());
0331       out.set_py(momentum.y());
0332       out.set_pz(momentum.z());
0333       auto sourceLink = state.getUncalibratedSourceLink().template get<ActsSourceLink>();
0334       out.set_cluskey(sourceLink.cluskey());
0335
0336       /// covariance
0337       const auto globalCov = rotateActsCovToSvtxTrack(params);
0338       for (int i = 0; i < 6; ++i) {
0339         for (int j = 0; j < 6; ++j)
0340       { out.set_error(i, j, globalCov(i,j)); }
0341 }
0342
0343       // print
0344       if(m_verbosity > 20)
0345       {
0346         std::cout << " inserting state with x,y,z ="
0347           << " " << global.x() /  Acts::UnitConstants::cm
0348           << " " << global.y() /  Acts::UnitConstants::cm
0349           << " " << global.z() /  Acts::UnitConstants::cm
0350       << " " << " localX, localY "
0351       << " " << localX / Acts::UnitConstants::cm
0352       << " " << localY / Acts::UnitConstants::cm
0353       << " pathlength " << pathlength
0354           << " momentum px,py,pz = " <<  momentum.x() << "  " <<  momentum.y() << "  " << momentum.y()
0355           << std::endl
0356           << "covariance " << globalCov << std::endl;
0357       }
0358
0359       svtxTrack->insert_state(&out);
0360
0361       return true; });
0362
0363   return;
0364 }