MkFitCore/standalone/ConformalUtilsMPlex.cc

0001 #include "ConformalUtilsMPlex.h"
0002 #include "RecoTracker/MkFitCore/standalone/ConfigStandalone.h"
0003 #include "RecoTracker/MkFitCore/interface/Track.h"
0004 #include "RecoTracker/MkFitCore/interface/Hit.h"
0005
0006 //#define DEBUG
0007 #include "RecoTracker/MkFitCore/src/Debug.h"
0008
0009 /* From MkFitter.h/.cc
0010 // ----------------
0011   void ConformalFitTracks(bool fitting, int beg, int end);
0012 // ----------------
0013   void MkFitter::ConformalFitTracks(bool fitting, int beg, int end) {
0014     // bool fitting to determine to use fitting CF error widths
0015     // in reality, this is depedent on hits used to make pulls
0016     // could consider writing an array for widths for a given hit combo
0017     // to give precise widths --> then would drop boolean
0018     // also used to determine which hits to use
0019
0020     int front, middle, back;
0021
0022     // FIXME FITTING HITS --> assume one hit per layer and all layers found! BAD! Need vector of indices to do this right instead...
0023     // can always assume 0,1,2 for seeding --> triplets in forward direction
0024 #ifdef INWARDFIT
0025     front = (fitting ? Config::nLayers - 1
0026                      : 0);  // i.e. would rather have true option not hardcoded... but set by ACTUAL last hit found
0027     middle =
0028         (fitting ? (Config::nLayers - 1) / 2 : 1);  // same with this one... would rather middle hit be in the middle!
0029     back = (fitting ? 0 : 2);
0030 #else
0031     front = (fitting ? 0 : 0);
0032     middle = (fitting ? (Config::nLayers - 1) / 2 : 1);  // ditto above
0033     back = (fitting ? Config::nLayers - 1 : 2);          // yup...
0034 #endif
0035
0036     // write to iC --> next step will be a propagation no matter what
0037     conformalFitMPlex(fitting, Label, Err[iC], Par[iC], msPar[front], msPar[middle], msPar[back]);
0038
0039     // need to set most off-diagonal elements in unc. to zero, inflate all other elements;
0040     if (fitting) {
0041       using idx_t = Matriplex::idx_t;
0042       const idx_t N = NN;
0043 #pragma omp simd
0044       for (int n = 0; n < N; ++n) {
0045         Err[iC].At(n, 0, 0) = Err[iC].constAt(n, 0, 0) * Config::blowupfit;
0046         Err[iC].At(n, 0, 1) = Err[iC].constAt(n, 0, 1) * Config::blowupfit;
0047         Err[iC].At(n, 1, 0) = Err[iC].constAt(n, 1, 0) * Config::blowupfit;
0048         Err[iC].At(n, 1, 1) = Err[iC].constAt(n, 1, 1) * Config::blowupfit;
0049         Err[iC].At(n, 2, 2) = Err[iC].constAt(n, 2, 2) * Config::blowupfit;
0050         Err[iC].At(n, 3, 3) = Err[iC].constAt(n, 3, 3) * Config::blowupfit;
0051         Err[iC].At(n, 4, 4) = Err[iC].constAt(n, 4, 4) * Config::blowupfit;
0052         Err[iC].At(n, 5, 5) = Err[iC].constAt(n, 5, 5) * Config::blowupfit;
0053
0054         Err[iC].At(n, 0, 2) = 0.0f;
0055         Err[iC].At(n, 0, 3) = 0.0f;
0056         Err[iC].At(n, 0, 4) = 0.0f;
0057         Err[iC].At(n, 0, 5) = 0.0f;
0058         Err[iC].At(n, 1, 2) = 0.0f;
0059         Err[iC].At(n, 1, 3) = 0.0f;
0060         Err[iC].At(n, 1, 4) = 0.0f;
0061         Err[iC].At(n, 1, 5) = 0.0f;
0062         Err[iC].At(n, 2, 0) = 0.0f;
0063         Err[iC].At(n, 2, 1) = 0.0f;
0064         Err[iC].At(n, 2, 3) = 0.0f;
0065         Err[iC].At(n, 2, 4) = 0.0f;
0066         Err[iC].At(n, 2, 5) = 0.0f;
0067         Err[iC].At(n, 3, 0) = 0.0f;
0068         Err[iC].At(n, 3, 1) = 0.0f;
0069         Err[iC].At(n, 3, 2) = 0.0f;
0070         Err[iC].At(n, 3, 4) = 0.0f;
0071         Err[iC].At(n, 3, 5) = 0.0f;
0072         Err[iC].At(n, 4, 0) = 0.0f;
0073         Err[iC].At(n, 4, 1) = 0.0f;
0074         Err[iC].At(n, 4, 2) = 0.0f;
0075         Err[iC].At(n, 4, 3) = 0.0f;
0076         Err[iC].At(n, 4, 5) = 0.0f;
0077         Err[iC].At(n, 5, 0) = 0.0f;
0078         Err[iC].At(n, 5, 1) = 0.0f;
0079         Err[iC].At(n, 5, 2) = 0.0f;
0080         Err[iC].At(n, 5, 3) = 0.0f;
0081         Err[iC].At(n, 5, 4) = 0.0f;
0082       }
0083     }
0084   }
0085 */
0086
0087 namespace mkfit {
0088
0089   inline void CFMap(const MPlexHH& A, const MPlexHV& B, MPlexHV& C) {
0090     using idx_t = Matriplex::idx_t;
0091
0092     // C = A * B, C is 3x1, A is 3x3 , B is 3x1
0093
0094     typedef float T;
0095     typedef float Tv;
0096     const idx_t N = NN;
0097
0098     const T* a = A.fArray;
0099     ASSUME_ALIGNED(a, 64);
0100     const Tv* b = B.fArray;
0101     ASSUME_ALIGNED(b, 64);
0102     Tv* c = C.fArray;
0103     ASSUME_ALIGNED(c, 64);
0104
0105 #include "RecoTracker/MkFitCore/standalone/CFMatrix33Vector3.ah"
0106   }
0107
0108   //M. Hansroul, H. Jeremie and D. Savard, NIM A 270 (1988) 498
0109   //http://www.sciencedirect.com/science/article/pii/016890028890722X
0110
0111   void conformalFitMPlex(bool fitting,
0112                          MPlexQI seedID,
0113                          MPlexLS& outErr,
0114                          MPlexLV& outPar,
0115                          const MPlexHV& msPar0,
0116                          const MPlexHV& msPar1,
0117                          const MPlexHV& msPar2) {
0118     bool debug(false);
0119
0120     using idx_t = Matriplex::idx_t;
0121     const idx_t N = NN;
0122
0123     // Store positions in mplex vectors... could consider storing in a 3x3 matrix, too
0124     MPlexHV x, y, z, r2;
0125 #pragma omp simd
0126     for (int n = 0; n < N; ++n) {
0127       x.At(n, 0, 0) = msPar0.constAt(n, 0, 0);
0128       x.At(n, 1, 0) = msPar1.constAt(n, 0, 0);
0129       x.At(n, 2, 0) = msPar2.constAt(n, 0, 0);
0130
0131       y.At(n, 0, 0) = msPar0.constAt(n, 1, 0);
0132       y.At(n, 1, 0) = msPar1.constAt(n, 1, 0);
0133       y.At(n, 2, 0) = msPar2.constAt(n, 1, 0);
0134
0135       z.At(n, 0, 0) = msPar0.constAt(n, 2, 0);
0136       z.At(n, 1, 0) = msPar1.constAt(n, 2, 0);
0137       z.At(n, 2, 0) = msPar2.constAt(n, 2, 0);
0138
0139       for (int i = 0; i < 3; ++i) {
0140         r2.At(n, i, 0) = getRad2(x.constAt(n, i, 0), y.constAt(n, i, 0));
0141       }
0142     }
0143
0144     // Start setting the output parameters
0145 #pragma omp simd
0146     for (int n = 0; n < N; ++n) {
0147       outPar.At(n, 0, 0) = x.constAt(n, 0, 0);
0148       outPar.At(n, 1, 0) = y.constAt(n, 0, 0);
0149       outPar.At(n, 2, 0) = z.constAt(n, 0, 0);
0150     }
0151
0152     // Use r-phi smearing to set initial error estimation for positions
0153     // trackStates already initialized to identity for seeding ... don't store off-diag 0's, zero's for fitting set outside CF
0154 #pragma omp simd
0155     for (int n = 0; n < N; ++n) {
0156       const float varPhi = Config::varXY / r2.constAt(n, 0, 0);
0157       const float invvarR2 = Config::varR / r2.constAt(n, 0, 0);
0158
0159       outErr.At(n, 0, 0) =
0160           x.constAt(n, 0, 0) * x.constAt(n, 0, 0) * invvarR2 + y.constAt(n, 0, 0) * y.constAt(n, 0, 0) * varPhi;
0161       outErr.At(n, 0, 1) = x.constAt(n, 0, 0) * y.constAt(n, 0, 0) * (invvarR2 - varPhi);
0162
0163       outErr.At(n, 1, 0) = outErr.constAt(n, 0, 1);
0164       outErr.At(n, 1, 1) =
0165           y.constAt(n, 0, 0) * y.constAt(n, 0, 0) * invvarR2 + x.constAt(n, 0, 0) * x.constAt(n, 0, 0) * varPhi;
0166
0167       outErr.At(n, 2, 2) = Config::varZ;
0168     }
0169
0170     MPlexQF initPhi;
0171     MPlexQI xtou;  // bool to determine "split space", i.e. map x to u or v
0172 #pragma omp simd
0173     for (int n = 0; n < N; ++n) {
0174       initPhi.At(n, 0, 0) = std::abs(getPhi(x.constAt(n, 0, 0), y.constAt(n, 0, 0)));
0175       xtou.At(n, 0, 0) =
0176           ((initPhi.constAt(n, 0, 0) < Const::PIOver4 || initPhi.constAt(n, 0, 0) > Const::PI3Over4) ? 1 : 0);
0177     }
0178
0179     MPlexHV u, v;
0180 #pragma omp simd
0181     for (int n = 0; n < N; ++n) {
0182       if (xtou.At(n, 0, 0))  // x mapped to u
0183       {
0184         for (int i = 0; i < 3; ++i) {
0185           u.At(n, i, 0) = x.constAt(n, i, 0) / r2.constAt(n, i, 0);
0186           v.At(n, i, 0) = y.constAt(n, i, 0) / r2.constAt(n, i, 0);
0187         }
0188       } else  // x mapped to v
0189       {
0190         for (int i = 0; i < 3; ++i) {
0191           u.At(n, i, 0) = y.constAt(n, i, 0) / r2.constAt(n, i, 0);
0192           v.At(n, i, 0) = x.constAt(n, i, 0) / r2.constAt(n, i, 0);
0193         }
0194       }
0195     }
0196
0197     MPlexHH A;
0198     //#pragma omp simd // triggers an internal compiler error with icc 18.0.2!
0199     for (int n = 0; n < N; ++n) {
0200       for (int i = 0; i < 3; ++i) {
0201         A.At(n, i, 0) = 1.0f;
0202         A.At(n, i, 1) = -u.constAt(n, i, 0);
0203         A.At(n, i, 2) = -u.constAt(n, i, 0) * u.constAt(n, i, 0);
0204       }
0205     }
0206     Matriplex::invertCramer(A);
0207     MPlexHV C;
0208     CFMap(A, v, C);
0209
0210     MPlexQF a, b;
0211 #pragma omp simd
0212     for (int n = 0; n < N; ++n) {
0213       b.At(n, 0, 0) = 1.0f / (2.0f * C.constAt(n, 0, 0));
0214       a.At(n, 0, 0) = b.constAt(n, 0, 0) * C.constAt(n, 1, 0);
0215     }
0216
0217     // constant used throughtout
0218     const float k = (Const::sol * Config::Bfield) / 100.0f;
0219
0220     MPlexQF vrx, vry, pT, px, py, pz;
0221 #pragma omp simd
0222     for (int n = 0; n < N; ++n) {
0223       vrx.At(n, 0, 0) =
0224           (xtou.constAt(n, 0, 0) ? x.constAt(n, 0, 0) - a.constAt(n, 0, 0) : x.constAt(n, 0, 0) - b.constAt(n, 0, 0));
0225       vry.At(n, 0, 0) =
0226           (xtou.constAt(n, 0, 0) ? y.constAt(n, 0, 0) - b.constAt(n, 0, 0) : y.constAt(n, 0, 0) - a.constAt(n, 0, 0));
0227       pT.At(n, 0, 0) = k * hipo(vrx.constAt(n, 0, 0), vry.constAt(n, 0, 0));
0228       px.At(n, 0, 0) = std::copysign(k * vry.constAt(n, 0, 0), x.constAt(n, 2, 0) - x.constAt(n, 0, 0));
0229       py.At(n, 0, 0) = std::copysign(k * vrx.constAt(n, 0, 0), y.constAt(n, 2, 0) - y.constAt(n, 0, 0));
0230       pz.At(n, 0, 0) = (pT.constAt(n, 0, 0) * (z.constAt(n, 2, 0) - z.constAt(n, 0, 0))) /
0231                        hipo((x.constAt(n, 2, 0) - x.constAt(n, 0, 0)), (y.constAt(n, 2, 0) - y.constAt(n, 0, 0)));
0232     }
0233
0234 #pragma omp simd
0235     for (int n = 0; n < N; ++n) {
0236       outPar.At(n, 3, 0) = 1.0f / pT.constAt(n, 0, 0);
0237       outPar.At(n, 4, 0) = getPhi(px.constAt(n, 0, 0), py.constAt(n, 0, 0));
0238       outPar.At(n, 5, 0) = getTheta(pT.constAt(n, 0, 0), pz.constAt(n, 0, 0));
0239 #ifdef INWARDFIT  // arctan is odd, so pz -> -pz means theta -> -theta
0240       if (fitting)
0241         outPar.At(n, 5, 0) *= -1.0f;
0242 #endif
0243     }
0244
0245 #pragma omp simd
0246     for (int n = 0; n < N; ++n) {
0247       outErr.At(n, 3, 3) =
0248           (fitting ? Config::ptinverr049 * Config::ptinverr049 : Config::ptinverr012 * Config::ptinverr012);
0249       outErr.At(n, 4, 4) = (fitting ? Config::phierr049 * Config::phierr049 : Config::phierr012 * Config::phierr012);
0250       outErr.At(n, 5, 5) =
0251           (fitting ? Config::thetaerr049 * Config::thetaerr049 : Config::thetaerr012 * Config::thetaerr012);
0252     }
0253
0254     if (debug && g_debug) {
0255       for (int n = 0; n < N; ++n) {
0256         dprintf("afterCF seedID: %1u \n", seedID.constAt(n, 0, 0));
0257         // do a dumb copy out
0258         TrackState updatedState;
0259         for (int i = 0; i < 6; i++) {
0260           updatedState.parameters[i] = outPar.constAt(n, i, 0);
0261           for (int j = 0; j < 6; j++) {
0262             updatedState.errors[i][j] = outErr.constAt(n, i, j);
0263           }
0264         }
0265
0266         dcall(print("CCS", updatedState));
0267         updatedState.convertFromCCSToCartesian();
0268         dcall(print("Pol", updatedState));
0269         dprint("--------------------------------");
0270       }
0271     }
0272   }
0273
0274 }  // end namespace mkfit