packages/trackreco/ActsAlignmentStates.cc

0001 #include "ActsAlignmentStates.h"
0002 #include "ActsPropagator.h"
0003
0004 #include <phool/getClass.h>
0005 #include <phool/PHCompositeNode.h>
0006
0007 #include <trackbase/ActsGeometry.h>
0008 #include <trackbase/ActsSourceLink.h>
0009 #include <trackbase/TpcDefs.h>
0010 #include <trackbase/TrkrCluster.h>
0011 #include <trackbase/TrkrClusterContainer.h>
0012 #include <trackbase/TrkrDefs.h>
0013
0014 #include <trackbase_historic/SvtxAlignmentState.h>
0015 #include <trackbase_historic/SvtxAlignmentStateMap.h>
0016 #include <trackbase_historic/SvtxAlignmentState_v1.h>
0017 #include <trackbase_historic/SvtxTrack.h>
0018
0019 #include <Acts/Definitions/Units.hpp>
0020 #include <Acts/EventData/MeasurementHelpers.hpp>
0021 #include <Acts/EventData/MultiTrajectory.hpp>
0022 #include <Acts/EventData/MultiTrajectoryHelpers.hpp>
0023 #include <Acts/Surfaces/Surface.hpp>
0024
0025 namespace
0026 {
0027   template <class T>
0028   inline constexpr T square(const T& x)
0029   {
0030     return x * x;
0031   }
0032   template <class T>
0033   inline constexpr T get_r(const T& x, const T& y)
0034   {
0035     return std::sqrt(square(x) + square(y));
0036   }
0037 }  // namespace
0038
0039 //_________________________________________________________________
0040 void ActsAlignmentStates::loadNodes( PHCompositeNode* topNode )
0041 {
0042   // load all nodes relevant to global position wrapper
0043   m_globalPositionWrapper.loadNodes(topNode);
0044
0045   // geometry
0046   m_tGeometry = findNode::getClass<ActsGeometry>(topNode, "ActsGeometry");
0047
0048   // cluster container
0049   m_clusterMap = findNode::getClass<TrkrClusterContainer>(topNode, "TRKR_CLUSTER");
0050
0051   // state map
0052   m_alignmentStateMap = findNode::getClass<SvtxAlignmentStateMap>(topNode, m_alignmentStateMapName);
0053
0054 }
0055
0056 //_________________________________________________________________
0057 void ActsAlignmentStates::fillAlignmentStateMap(
0058   const ActsTrackFittingAlgorithm::TrackContainer& tracks,
0059   const std::vector<Acts::MultiTrajectoryTraits::IndexType>& tips,
0060   SvtxTrack* track,
0061   const ActsTrackFittingAlgorithm::MeasurementContainer& measurements)
0062 {
0063   const auto& mj = tracks.trackStateContainer();
0064   const auto& trackTip = tips.front();
0065   const auto crossing = track->get_silicon_seed()->get_crossing();
0066
0067   ActsPropagator propagator(m_tGeometry);
0068   std::istringstream stringline(m_fieldMap);
0069   double fieldstrength = std::numeric_limits<double>::quiet_NaN();
0070   stringline >> fieldstrength;
0071   if (!stringline.fail())
0072   {
0073     propagator.constField();
0074     propagator.setConstFieldValue(fieldstrength);
0075   }
0076
0077   SvtxAlignmentStateMap::StateVec statevec;
0078   if (m_verbosity > 2)
0079   {
0080     std::cout << "Beginning alignment state creation for track "
0081               << track->get_id() << std::endl;
0082   }
0083
0084   std::vector<Acts::BoundIndices> indices{Acts::eBoundLoc0, Acts::eBoundLoc1, Acts::eBoundPhi, Acts::eBoundTheta, Acts::eBoundQOverP, Acts::eBoundTime};
0085
0086   auto silseed = track->get_silicon_seed();
0087   int nmaps = 0;
0088   int nintt = 0;
0089   for (auto iter = silseed->begin_cluster_keys();
0090        iter != silseed->end_cluster_keys();
0091        ++iter)
0092   {
0093     TrkrDefs::cluskey ckey = *iter;
0094     if (TrkrDefs::getTrkrId(ckey) == TrkrDefs::TrkrId::mvtxId)
0095     {
0096       nmaps++;
0097     }
0098     if (TrkrDefs::getTrkrId(ckey) == TrkrDefs::TrkrId::inttId)
0099     {
0100       nintt++;
0101     }
0102   }
0103
0104   if (nmaps < 2 or nintt < 2)
0105   {
0106     return;
0107   }
0108
0109   //! make sure the track was fully fit through the mvtx
0110   SvtxTrackState* firststate = (*std::next(track->begin_states(), 1)).second;
0111   if (get_r(firststate->get_x(), firststate->get_y()) > 5.)
0112   {
0113     return;
0114   }
0115
0116   mj.visitBackwards(trackTip, [&](const auto& state)
0117                     {
0118     /// Collect only track states which were used in smoothing of KF and are measurements
0119     if (not state.hasSmoothed() or
0120         not state.typeFlags().test(Acts::TrackStateFlag::MeasurementFlag))
0121     {
0122       return true;
0123     }
0124
0125     const auto& surface = state.referenceSurface();
0126     auto sl = state.getUncalibratedSourceLink().template get<ActsSourceLink>();
0127     auto ckey = sl.cluskey();
0128     Acts::Vector2 localMeas = Acts::Vector2::Zero();
0129     /// get the local measurement that acts used
0130     std::visit([&](const auto& meas) {
0131     localMeas(0) = meas.parameters()[0];
0132     localMeas(1) = meas.parameters()[1];
0133       }, measurements[sl.index()]);
0134
0135     if (m_verbosity > 2)
0136     {
0137       std::cout << "sl index and ckey " << sl.index() << ", "
0138         << sl.cluskey() << " with local position "
0139         << localMeas.transpose() << std::endl;
0140     }
0141
0142     auto clus = m_clusterMap->findCluster(ckey);
0143
0144     // local state vector
0145     const Acts::Vector2 localState = state.effectiveProjector() * state.smoothed();
0146
0147     // Local residual between measurement and smoothed Acts state
0148     const Acts::Vector2 localResidual = localMeas - localState;
0149
0150     // get cluster global position, in acts units
0151     const Acts::Vector3 clusGlobal = m_globalPositionWrapper.getGlobalPositionDistortionCorrected(ckey, clus, crossing)*Acts::UnitConstants::cm;
0152
0153     const Acts::FreeVector globalStateParams = Acts::transformBoundToFreeParameters(surface, m_tGeometry->geometry().getGeoContext(), state.smoothed());
0154     const Acts::Vector3 stateGlobal = globalStateParams.segment<3>(Acts::eFreePos0);
0155
0156     const Acts::Vector3 clus_sigma =
0157     {
0158       clus->getRPhiError() / sqrt(2) * Acts::UnitConstants::cm,
0159       clus->getRPhiError() / sqrt(2) * Acts::UnitConstants::cm,
0160       clus->getZError() * Acts::UnitConstants::cm
0161     };
0162
0163     if (m_verbosity > 2)
0164     {
0165       std::cout << "clus global is " << clusGlobal.transpose() << std::endl
0166                 << "state global is " << stateGlobal.transpose() << std::endl;
0167       std::cout << "   clus errors are " << clus_sigma.transpose() << std::endl;
0168       std::cout << "   clus loc is " << localMeas.transpose() << std::endl;
0169       std::cout << "   state loc is " << localState << std::endl;
0170       std::cout << "   local residual is " << localResidual.transpose() << std::endl;
0171     }
0172
0173     // Get the derivative of alignment (global) parameters w.r.t. measurement or residual
0174     /// The local bound parameters still have access to global phi/theta
0175     const double phi = state.smoothed()[Acts::eBoundPhi];
0176     const double theta = state.smoothed()[Acts::eBoundTheta];
0177
0178     Acts::Vector3 tangent = Acts::makeDirectionFromPhiTheta(phi,theta);
0179
0180     // opposite convention for coordinates in Acts
0181     tangent *= -1;
0182
0183     if(m_verbosity > 2)
0184     {
0185       std::cout << "tangent vector to track state is " << tangent.transpose() << std::endl;
0186     }
0187
0188     const auto projxy = get_projectionXY(surface, tangent);
0189
0190     const Acts::Vector3 sensorCenter = surface.center(m_tGeometry->geometry().getGeoContext());
0191     const Acts::Vector3 OM = stateGlobal - sensorCenter;
0192
0193     const auto globDeriv = makeGlobalDerivatives(OM, projxy);
0194
0195     if(m_verbosity > 2)
0196       {
0197     std::cout << "   global deriv calcs" << std::endl
0198           << "stateGlobal: " << stateGlobal.transpose()
0199           << ", sensor center " << sensorCenter.transpose() << std::endl
0200           << ", OM " << OM.transpose() << std::endl << "   projxy "
0201           << projxy.first.transpose() << ", "
0202           << projxy.second.transpose() << std::endl
0203           << "global derivatives " << std::endl << globDeriv << std::endl;
0204       }
0205
0206     //! this is the derivative of the state wrt to Acts track parameters
0207     //! e.g. (d_0, z_0, phi, theta, q/p, t)
0208     auto localDeriv = state.effectiveProjector() * state.jacobian();
0209     if(m_verbosity > 2)
0210       {
0211     std::cout << "local deriv " << std::endl << localDeriv << std::endl;
0212       }
0213
0214     auto svtxstate = std::make_unique<SvtxAlignmentState_v1>();
0215
0216     svtxstate->set_residual(localResidual);
0217     svtxstate->set_local_derivative_matrix(localDeriv);
0218     svtxstate->set_global_derivative_matrix(globDeriv);
0219     svtxstate->set_cluster_key(ckey);
0220
0221     statevec.push_back(svtxstate.release());
0222     return true; });
0223
0224   if (m_verbosity > 2)
0225   {
0226     std::cout << "Inserting track " << track->get_id() << " with nstates "
0227               << statevec.size() << std::endl;
0228   }
0229
0230   m_alignmentStateMap->insertWithKey(track->get_id(), statevec);
0231
0232   return;
0233 }
0234
0235 SvtxAlignmentState::GlobalMatrix
0236 ActsAlignmentStates::makeGlobalDerivatives(const Acts::Vector3& OM,
0237                                            const std::pair<Acts::Vector3, Acts::Vector3>& projxy)
0238 {
0239   SvtxAlignmentState::GlobalMatrix globalder = SvtxAlignmentState::GlobalMatrix::Zero();
0240   Acts::SquareMatrix3 unit = Acts::SquareMatrix3::Identity();
0241
0242   //! x residual rotations
0243   globalder(0, 0) = ((unit.col(0)).cross(OM)).dot(projxy.first);
0244   globalder(0, 1) = ((unit.col(1)).cross(OM)).dot(projxy.first);
0245   globalder(0, 2) = ((unit.col(2)).cross(OM)).dot(projxy.first);
0246   //! x residual translations
0247   globalder(0, 3) = unit.col(0).dot(projxy.first);
0248   globalder(0, 4) = unit.col(1).dot(projxy.first);
0249   globalder(0, 5) = unit.col(2).dot(projxy.first);
0250
0251   //! y residual rotations
0252   globalder(1, 0) = ((unit.col(0)).cross(OM)).dot(projxy.second);
0253   globalder(1, 1) = ((unit.col(1)).cross(OM)).dot(projxy.second);
0254   globalder(1, 2) = ((unit.col(2)).cross(OM)).dot(projxy.second);
0255   //! y residual translations
0256   globalder(1, 3) = unit.col(0).dot(projxy.second);
0257   globalder(1, 4) = unit.col(1).dot(projxy.second);
0258   globalder(1, 5) = unit.col(2).dot(projxy.second);
0259
0260   return globalder;
0261 }
0262
0263 //______________________________________________________
0264 std::pair<Acts::Vector3, Acts::Vector3> ActsAlignmentStates::get_projectionXY(const Acts::Surface& surface, const Acts::Vector3& tangent)
0265 {
0266   Acts::Vector3 projx = Acts::Vector3::Zero();
0267   Acts::Vector3 projy = Acts::Vector3::Zero();
0268
0269   // get the plane of the surface
0270   Acts::Vector3 sensorCenter = surface.center(m_tGeometry->geometry().getGeoContext());
0271   // sensorNormal is the Z vector
0272   Acts::Vector3 Z = -surface.normal(m_tGeometry->geometry().getGeoContext(),
0273     Acts::Vector3(1,1,1), Acts::Vector3(1,1,1));
0274
0275   // get surface X and Y unit vectors in global frame
0276   // transform Xlocal = 1.0 to global, subtract the surface center, normalize to 1
0277   Acts::Vector3 xloc(1.0, 0.0, 0.0);
0278   Acts::Vector3 xglob = surface.transform(m_tGeometry->geometry().getGeoContext()) * xloc;
0279
0280   Acts::Vector3 yloc(0.0, 1.0, 0.0);
0281   Acts::Vector3 yglob = surface.transform(m_tGeometry->geometry().getGeoContext()) * yloc;
0282
0283   Acts::Vector3 X = (xglob - sensorCenter) / (xglob - sensorCenter).norm();
0284   Acts::Vector3 Y = (yglob - sensorCenter) / (yglob - sensorCenter).norm();
0285
0286   projx = X - (tangent.dot(X) / tangent.dot(Z)) * Z;
0287   projy = Y - (tangent.dot(Y) / tangent.dot(Z)) * Z;
0288   if (m_verbosity > 2)
0289   {
0290     std::cout << "projxy " << projx.transpose() << ", " << projy.transpose() << std::endl;
0291   }
0292
0293   return std::make_pair(projx, projy);
0294 }