PixelTrackFitting/interface/FitUtils.h

0001 #ifndef RecoTracker_PixelTrackFitting_FitUtils_h
0002 #define RecoTracker_PixelTrackFitting_FitUtils_h
0003
0004 #include "DataFormats/Math/interface/choleskyInversion.h"
0005 #include "HeterogeneousCore/CUDAUtilities/interface/cuda_assert.h"
0006 #include "RecoTracker/PixelTrackFitting/interface/FitResult.h"
0007
0008 namespace riemannFit {
0009
0010   constexpr double epsilon = 1.e-4;  //!< used in numerical derivative (J2 in Circle_fit())
0011
0012   using VectorXd = Eigen::VectorXd;
0013   using MatrixXd = Eigen::MatrixXd;
0014   template <int N>
0015   using MatrixNd = Eigen::Matrix<double, N, N>;
0016   template <int N>
0017   using MatrixNplusONEd = Eigen::Matrix<double, N + 1, N + 1>;
0018   template <int N>
0019   using ArrayNd = Eigen::Array<double, N, N>;
0020   template <int N>
0021   using Matrix2Nd = Eigen::Matrix<double, 2 * N, 2 * N>;
0022   template <int N>
0023   using Matrix3Nd = Eigen::Matrix<double, 3 * N, 3 * N>;
0024   template <int N>
0025   using Matrix2xNd = Eigen::Matrix<double, 2, N>;
0026   template <int N>
0027   using Array2xNd = Eigen::Array<double, 2, N>;
0028   template <int N>
0029   using MatrixNx3d = Eigen::Matrix<double, N, 3>;
0030   template <int N>
0031   using MatrixNx5d = Eigen::Matrix<double, N, 5>;
0032   template <int N>
0033   using VectorNd = Eigen::Matrix<double, N, 1>;
0034   template <int N>
0035   using VectorNplusONEd = Eigen::Matrix<double, N + 1, 1>;
0036   template <int N>
0037   using Vector2Nd = Eigen::Matrix<double, 2 * N, 1>;
0038   template <int N>
0039   using Vector3Nd = Eigen::Matrix<double, 3 * N, 1>;
0040   template <int N>
0041   using RowVectorNd = Eigen::Matrix<double, 1, 1, N>;
0042   template <int N>
0043   using RowVector2Nd = Eigen::Matrix<double, 1, 2 * N>;
0044
0045   using Matrix2x3d = Eigen::Matrix<double, 2, 3>;
0046
0047   using Matrix3f = Eigen::Matrix3f;
0048   using Vector3f = Eigen::Vector3f;
0049   using Vector4f = Eigen::Vector4f;
0050   using Vector6f = Eigen::Matrix<double, 6, 1>;
0051
0052   template <class C>
0053   __host__ __device__ void printIt(C* m, const char* prefix = "") {
0054 #ifdef RFIT_DEBUG
0055     for (uint r = 0; r < m->rows(); ++r) {
0056       for (uint c = 0; c < m->cols(); ++c) {
0057         printf("%s Matrix(%d,%d) = %g\n", prefix, r, c, (*m)(r, c));
0058       }
0059     }
0060 #endif
0061   }
0062
0063   /*!
0064     \brief raise to square.
0065   */
0066   template <typename T>
0067   constexpr T sqr(const T a) {
0068     return a * a;
0069   }
0070
0071   /*!
0072     \brief Compute cross product of two 2D vector (assuming z component 0),
0073     returning z component of the result.
0074     \param a first 2D vector in the product.
0075     \param b second 2D vector in the product.
0076     \return z component of the cross product.
0077   */
0078
0079   __host__ __device__ inline double cross2D(const Vector2d& a, const Vector2d& b) {
0080     return a.x() * b.y() - a.y() * b.x();
0081   }
0082
0083   /*!
0084    *  load error in CMSSW format to our formalism
0085    *
0086    */
0087   template <typename M6xNf, typename M2Nd>
0088   __host__ __device__ void loadCovariance2D(M6xNf const& ge, M2Nd& hits_cov) {
0089     // Index numerology:
0090     // i: index of the hits/point (0,..,3)
0091     // j: index of space component (x,y,z)
0092     // l: index of space components (x,y,z)
0093     // ge is always in sync with the index i and is formatted as:
0094     // ge[] ==> [xx, xy, yy, xz, yz, zz]
0095     // in (j,l) notation, we have:
0096     // ge[] ==> [(0,0), (0,1), (1,1), (0,2), (1,2), (2,2)]
0097     // so the index ge_idx corresponds to the matrix elements:
0098     // | 0  1  3 |
0099     // | 1  2  4 |
0100     // | 3  4  5 |
0101     constexpr uint32_t hits_in_fit = M6xNf::ColsAtCompileTime;
0102     for (uint32_t i = 0; i < hits_in_fit; ++i) {
0103       {
0104         constexpr uint32_t ge_idx = 0, j = 0, l = 0;
0105         hits_cov(i + j * hits_in_fit, i + l * hits_in_fit) = ge.col(i)[ge_idx];
0106       }
0107       {
0108         constexpr uint32_t ge_idx = 2, j = 1, l = 1;
0109         hits_cov(i + j * hits_in_fit, i + l * hits_in_fit) = ge.col(i)[ge_idx];
0110       }
0111       {
0112         constexpr uint32_t ge_idx = 1, j = 1, l = 0;
0113         hits_cov(i + l * hits_in_fit, i + j * hits_in_fit) = hits_cov(i + j * hits_in_fit, i + l * hits_in_fit) =
0114             ge.col(i)[ge_idx];
0115       }
0116     }
0117   }
0118
0119   template <typename M6xNf, typename M3xNd>
0120   __host__ __device__ void loadCovariance(M6xNf const& ge, M3xNd& hits_cov) {
0121     // Index numerology:
0122     // i: index of the hits/point (0,..,3)
0123     // j: index of space component (x,y,z)
0124     // l: index of space components (x,y,z)
0125     // ge is always in sync with the index i and is formatted as:
0126     // ge[] ==> [xx, xy, yy, xz, yz, zz]
0127     // in (j,l) notation, we have:
0128     // ge[] ==> [(0,0), (0,1), (1,1), (0,2), (1,2), (2,2)]
0129     // so the index ge_idx corresponds to the matrix elements:
0130     // | 0  1  3 |
0131     // | 1  2  4 |
0132     // | 3  4  5 |
0133     constexpr uint32_t hits_in_fit = M6xNf::ColsAtCompileTime;
0134     for (uint32_t i = 0; i < hits_in_fit; ++i) {
0135       {
0136         constexpr uint32_t ge_idx = 0, j = 0, l = 0;
0137         hits_cov(i + j * hits_in_fit, i + l * hits_in_fit) = ge.col(i)[ge_idx];
0138       }
0139       {
0140         constexpr uint32_t ge_idx = 2, j = 1, l = 1;
0141         hits_cov(i + j * hits_in_fit, i + l * hits_in_fit) = ge.col(i)[ge_idx];
0142       }
0143       {
0144         constexpr uint32_t ge_idx = 5, j = 2, l = 2;
0145         hits_cov(i + j * hits_in_fit, i + l * hits_in_fit) = ge.col(i)[ge_idx];
0146       }
0147       {
0148         constexpr uint32_t ge_idx = 1, j = 1, l = 0;
0149         hits_cov(i + l * hits_in_fit, i + j * hits_in_fit) = hits_cov(i + j * hits_in_fit, i + l * hits_in_fit) =
0150             ge.col(i)[ge_idx];
0151       }
0152       {
0153         constexpr uint32_t ge_idx = 3, j = 2, l = 0;
0154         hits_cov(i + l * hits_in_fit, i + j * hits_in_fit) = hits_cov(i + j * hits_in_fit, i + l * hits_in_fit) =
0155             ge.col(i)[ge_idx];
0156       }
0157       {
0158         constexpr uint32_t ge_idx = 4, j = 2, l = 1;
0159         hits_cov(i + l * hits_in_fit, i + j * hits_in_fit) = hits_cov(i + j * hits_in_fit, i + l * hits_in_fit) =
0160             ge.col(i)[ge_idx];
0161       }
0162     }
0163   }
0164
0165   /*!
0166     \brief Transform circle parameter from (X0,Y0,R) to (phi,Tip,p_t) and
0167     consequently covariance matrix.
0168     \param circle_uvr parameter (X0,Y0,R), covariance matrix to
0169     be transformed and particle charge.
0170     \param B magnetic field in Gev/cm/c unit.
0171     \param error flag for errors computation.
0172   */
0173   __host__ __device__ inline void par_uvrtopak(CircleFit& circle, const double B, const bool error) {
0174     Vector3d par_pak;
0175     const double temp0 = circle.par.head(2).squaredNorm();
0176     const double temp1 = sqrt(temp0);
0177     par_pak << atan2(circle.qCharge * circle.par(0), -circle.qCharge * circle.par(1)),
0178         circle.qCharge * (temp1 - circle.par(2)), circle.par(2) * B;
0179     if (error) {
0180       const double temp2 = sqr(circle.par(0)) * 1. / temp0;
0181       const double temp3 = 1. / temp1 * circle.qCharge;
0182       Matrix3d j4Mat;
0183       j4Mat << -circle.par(1) * temp2 * 1. / sqr(circle.par(0)), temp2 * 1. / circle.par(0), 0., circle.par(0) * temp3,
0184           circle.par(1) * temp3, -circle.qCharge, 0., 0., B;
0185       circle.cov = j4Mat * circle.cov * j4Mat.transpose();
0186     }
0187     circle.par = par_pak;
0188   }
0189
0190   /*!
0191     \brief Transform circle parameter from (X0,Y0,R) to (phi,Tip,q/R) and
0192     consequently covariance matrix.
0193     \param circle_uvr parameter (X0,Y0,R), covariance matrix to
0194     be transformed and particle charge.
0195   */
0196   __host__ __device__ inline void fromCircleToPerigee(CircleFit& circle) {
0197     Vector3d par_pak;
0198     const double temp0 = circle.par.head(2).squaredNorm();
0199     const double temp1 = sqrt(temp0);
0200     par_pak << atan2(circle.qCharge * circle.par(0), -circle.qCharge * circle.par(1)),
0201         circle.qCharge * (temp1 - circle.par(2)), circle.qCharge / circle.par(2);
0202
0203     const double temp2 = sqr(circle.par(0)) * 1. / temp0;
0204     const double temp3 = 1. / temp1 * circle.qCharge;
0205     Matrix3d j4Mat;
0206     j4Mat << -circle.par(1) * temp2 * 1. / sqr(circle.par(0)), temp2 * 1. / circle.par(0), 0., circle.par(0) * temp3,
0207         circle.par(1) * temp3, -circle.qCharge, 0., 0., -circle.qCharge / (circle.par(2) * circle.par(2));
0208     circle.cov = j4Mat * circle.cov * j4Mat.transpose();
0209
0210     circle.par = par_pak;
0211   }
0212
0213   // transformation between the "perigee" to cmssw localcoord frame
0214   // the plane of the latter is the perigee plane...
0215   // from   //!<(phi,Tip,q/pt,cotan(theta)),Zip)
0216   // to q/p,dx/dz,dy/dz,x,z
0217   template <typename VI5, typename MI5, typename VO5, typename MO5>
0218   __host__ __device__ inline void transformToPerigeePlane(VI5 const& ip, MI5 const& icov, VO5& op, MO5& ocov) {
0219     auto sinTheta2 = 1. / (1. + ip(3) * ip(3));
0220     auto sinTheta = std::sqrt(sinTheta2);
0221     auto cosTheta = ip(3) * sinTheta;
0222
0223     op(0) = sinTheta * ip(2);
0224     op(1) = 0.;
0225     op(2) = -ip(3);
0226     op(3) = ip(1);
0227     op(4) = -ip(4);
0228
0229     Matrix5d jMat = Matrix5d::Zero();
0230
0231     jMat(0, 2) = sinTheta;
0232     jMat(0, 3) = -sinTheta2 * cosTheta * ip(2);
0233     jMat(1, 0) = 1.;
0234     jMat(2, 3) = -1.;
0235     jMat(3, 1) = 1.;
0236     jMat(4, 4) = -1;
0237
0238     ocov = jMat * icov * jMat.transpose();
0239   }
0240
0241 }  // namespace riemannFit
0242
0243 #endif  // RecoTracker_PixelTrackFitting_FitUtils_h