PixelTrackFitting/interface/BrokenLine.h

0001 #ifndef RecoTracker_PixelTrackFitting_BrokenLine_h
0002 #define RecoTracker_PixelTrackFitting_BrokenLine_h
0003
0004 #include <Eigen/Eigenvalues>
0005
0006 #include "RecoTracker/PixelTrackFitting/interface/FitUtils.h"
0007
0008 namespace brokenline {
0009
0010   //!< Karimäki's parameters: (phi, d, k=1/R)
0011   /*!< covariance matrix: \n
0012     |cov(phi,phi)|cov( d ,phi)|cov( k ,phi)| \n
0013     |cov(phi, d )|cov( d , d )|cov( k , d )| \n
0014     |cov(phi, k )|cov( d , k )|cov( k , k )| \n
0015     as defined in Karimäki V., 1990, Effective circle fitting for particle trajectories,
0016     Nucl. Instr. and Meth. A305 (1991) 187.
0017   */
0018   using karimaki_circle_fit = riemannFit::CircleFit;
0019
0020   /*!
0021     \brief data needed for the Broken Line fit procedure.
0022   */
0023   template <int n>
0024   struct PreparedBrokenLineData {
0025     int qCharge;                          //!< particle charge
0026     riemannFit::Matrix2xNd<n> radii;      //!< xy data in the system in which the pre-fitted center is the origin
0027     riemannFit::VectorNd<n> sTransverse;  //!< total distance traveled in the transverse plane
0028                                           //   starting from the pre-fitted closest approach
0029     riemannFit::VectorNd<n> sTotal;       //!< total distance traveled (three-dimensional)
0030     riemannFit::VectorNd<n> zInSZplane;   //!< orthogonal coordinate to the pre-fitted line in the sz plane
0031     riemannFit::VectorNd<n> varBeta;      //!< kink angles in the SZ plane
0032   };
0033
0034   /*!
0035     \brief Computes the Coulomb multiple scattering variance of the planar angle.
0036
0037     \param length length of the track in the material.
0038     \param bField magnetic field in Gev/cm/c.
0039     \param radius radius of curvature (needed to evaluate p).
0040     \param layer denotes which of the four layers of the detector is the endpoint of the
0041    *             multiple scattered track. For example, if Layer=3, then the particle has
0042    *             just gone through the material between the second and the third layer.
0043
0044     \todo add another Layer variable to identify also the start point of the track,
0045    *      so if there are missing hits or multiple hits, the part of the detector that
0046    *      the particle has traversed can be exactly identified.
0047
0048     \warning the formula used here assumes beta=1, and so neglects the dependence
0049    *         of theta_0 on the mass of the particle at fixed momentum.
0050
0051     \return the variance of the planar angle ((theta_0)^2 /3).
0052   */
0053   __host__ __device__ inline double multScatt(
0054       const double& length, const double bField, const double radius, int layer, double slope) {
0055     // limit R to 20GeV...
0056     auto pt2 = std::min(20., bField * radius);
0057     pt2 *= pt2;
0058     constexpr double inv_X0 = 0.06 / 16.;  //!< inverse of radiation length of the material in cm
0059     //if(Layer==1) XXI_0=0.06/16.;
0060     // else XXI_0=0.06/16.;
0061     //XX_0*=1;
0062
0063     //! number between 1/3 (uniform material) and 1 (thin scatterer) to be manually tuned
0064     constexpr double geometry_factor = 0.7;
0065     constexpr double fact = geometry_factor * riemannFit::sqr(13.6 / 1000.);
0066     return fact / (pt2 * (1. + riemannFit::sqr(slope))) * (std::abs(length) * inv_X0) *
0067            riemannFit::sqr(1. + 0.038 * log(std::abs(length) * inv_X0));
0068   }
0069
0070   /*!
0071     \brief Computes the 2D rotation matrix that transforms the line y=slope*x into the line y=0.
0072
0073     \param slope tangent of the angle of rotation.
0074
0075     \return 2D rotation matrix.
0076   */
0077   __host__ __device__ inline riemannFit::Matrix2d rotationMatrix(double slope) {
0078     riemannFit::Matrix2d rot;
0079     rot(0, 0) = 1. / sqrt(1. + riemannFit::sqr(slope));
0080     rot(0, 1) = slope * rot(0, 0);
0081     rot(1, 0) = -rot(0, 1);
0082     rot(1, 1) = rot(0, 0);
0083     return rot;
0084   }
0085
0086   /*!
0087     \brief Changes the Karimäki parameters (and consequently their covariance matrix) under a
0088    *       translation of the coordinate system, such that the old origin has coordinates (x0,y0)
0089    *       in the new coordinate system. The formulas are taken from Karimäki V., 1990, Effective
0090    *       circle fitting for particle trajectories, Nucl. Instr. and Meth. A305 (1991) 187.
0091
0092     \param circle circle fit in the old coordinate system. circle.par(0) is phi, circle.par(1) is d and circle.par(2) is rho.
0093     \param x0 x coordinate of the translation vector.
0094     \param y0 y coordinate of the translation vector.
0095     \param jacobian passed by reference in order to save stack.
0096   */
0097   __host__ __device__ inline void translateKarimaki(karimaki_circle_fit& circle,
0098                                                     double x0,
0099                                                     double y0,
0100                                                     riemannFit::Matrix3d& jacobian) {
0101     // Avoid multiple access to the circle.par vector.
0102     using scalar = std::remove_reference<decltype(circle.par(0))>::type;
0103     scalar phi = circle.par(0);
0104     scalar dee = circle.par(1);
0105     scalar rho = circle.par(2);
0106
0107     // Avoid repeated trig. computations
0108     scalar sinPhi = sin(phi);
0109     scalar cosPhi = cos(phi);
0110
0111     // Intermediate computations for the circle parameters
0112     scalar deltaPara = x0 * cosPhi + y0 * sinPhi;
0113     scalar deltaOrth = x0 * sinPhi - y0 * cosPhi + dee;
0114     scalar tempSmallU = 1 + rho * dee;
0115     scalar tempC = -rho * y0 + tempSmallU * cosPhi;
0116     scalar tempB = rho * x0 + tempSmallU * sinPhi;
0117     scalar tempA = 2. * deltaOrth + rho * (riemannFit::sqr(deltaOrth) + riemannFit::sqr(deltaPara));
0118     scalar tempU = sqrt(1. + rho * tempA);
0119
0120     // Intermediate computations for the error matrix transform
0121     scalar xi = 1. / (riemannFit::sqr(tempB) + riemannFit::sqr(tempC));
0122     scalar tempV = 1. + rho * deltaOrth;
0123     scalar lambda = (0.5 * tempA) / (riemannFit::sqr(1. + tempU) * tempU);
0124     scalar mu = 1. / (tempU * (1. + tempU)) + rho * lambda;
0125     scalar zeta = riemannFit::sqr(deltaOrth) + riemannFit::sqr(deltaPara);
0126     jacobian << xi * tempSmallU * tempV, -xi * riemannFit::sqr(rho) * deltaOrth, xi * deltaPara,
0127         2. * mu * tempSmallU * deltaPara, 2. * mu * tempV, mu * zeta - lambda * tempA, 0, 0, 1.;
0128
0129     // translated circle parameters
0130     // phi
0131     circle.par(0) = atan2(tempB, tempC);
0132     // d
0133     circle.par(1) = tempA / (1 + tempU);
0134     // rho after translation. It is invariant, so noop
0135     // circle.par(2)= rho;
0136
0137     // translated error matrix
0138     circle.cov = jacobian * circle.cov * jacobian.transpose();
0139   }
0140
0141   /*!
0142     \brief Computes the data needed for the Broken Line fit procedure that are mainly common for the circle and the line fit.
0143
0144     \param hits hits coordinates.
0145     \param fast_fit pre-fit result in the form (X0,Y0,R,tan(theta)).
0146     \param bField magnetic field in Gev/cm/c.
0147     \param results PreparedBrokenLineData to be filled (see description of PreparedBrokenLineData).
0148   */
0149   template <typename M3xN, typename V4, int n>
0150   __host__ __device__ inline void prepareBrokenLineData(const M3xN& hits,
0151                                                         const V4& fast_fit,
0152                                                         const double bField,
0153                                                         PreparedBrokenLineData<n>& results) {
0154     riemannFit::Vector2d dVec;
0155     riemannFit::Vector2d eVec;
0156
0157     int mId = 1;
0158
0159     if constexpr (n > 3) {
0160       riemannFit::Vector2d middle = 0.5 * (hits.block(0, n - 1, 2, 1) + hits.block(0, 0, 2, 1));
0161       auto d1 = (hits.block(0, n / 2, 2, 1) - middle).squaredNorm();
0162       auto d2 = (hits.block(0, n / 2 - 1, 2, 1) - middle).squaredNorm();
0163       mId = d1 < d2 ? n / 2 : n / 2 - 1;
0164     }
0165
0166     dVec = hits.block(0, mId, 2, 1) - hits.block(0, 0, 2, 1);
0167     eVec = hits.block(0, n - 1, 2, 1) - hits.block(0, mId, 2, 1);
0168     results.qCharge = riemannFit::cross2D(dVec, eVec) > 0 ? -1 : 1;
0169
0170     const double slope = -results.qCharge / fast_fit(3);
0171
0172     riemannFit::Matrix2d rotMat = rotationMatrix(slope);
0173
0174     // calculate radii and s
0175     results.radii = hits.block(0, 0, 2, n) - fast_fit.head(2) * riemannFit::MatrixXd::Constant(1, n, 1);
0176     eVec = -fast_fit(2) * fast_fit.head(2) / fast_fit.head(2).norm();
0177     for (u_int i = 0; i < n; i++) {
0178       dVec = results.radii.block(0, i, 2, 1);
0179       results.sTransverse(i) = results.qCharge * fast_fit(2) *
0180                                atan2(riemannFit::cross2D(dVec, eVec), dVec.dot(eVec));  // calculates the arc length
0181     }
0182     riemannFit::VectorNd<n> zVec = hits.block(2, 0, 1, n).transpose();
0183
0184     //calculate sTotal and zVec
0185     riemannFit::Matrix2xNd<n> pointsSZ = riemannFit::Matrix2xNd<n>::Zero();
0186     for (u_int i = 0; i < n; i++) {
0187       pointsSZ(0, i) = results.sTransverse(i);
0188       pointsSZ(1, i) = zVec(i);
0189       pointsSZ.block(0, i, 2, 1) = rotMat * pointsSZ.block(0, i, 2, 1);
0190     }
0191     results.sTotal = pointsSZ.block(0, 0, 1, n).transpose();
0192     results.zInSZplane = pointsSZ.block(1, 0, 1, n).transpose();
0193
0194     //calculate varBeta
0195     results.varBeta(0) = results.varBeta(n - 1) = 0;
0196     for (u_int i = 1; i < n - 1; i++) {
0197       results.varBeta(i) = multScatt(results.sTotal(i + 1) - results.sTotal(i), bField, fast_fit(2), i + 2, slope) +
0198                            multScatt(results.sTotal(i) - results.sTotal(i - 1), bField, fast_fit(2), i + 1, slope);
0199     }
0200   }
0201
0202   /*!
0203     \brief Computes the n-by-n band matrix obtained minimizing the Broken Line's cost function w.r.t u.
0204    *       This is the whole matrix in the case of the line fit and the main n-by-n block in the case
0205    *       of the circle fit.
0206
0207     \param weights weights of the first part of the cost function, the one with the measurements
0208    *         and not the angles (\sum_{i=1}^n w*(y_i-u_i)^2).
0209     \param sTotal total distance traveled by the particle from the pre-fitted closest approach.
0210     \param varBeta kink angles' variance.
0211
0212     \return the n-by-n matrix of the linear system
0213   */
0214   template <int n>
0215   __host__ __device__ inline riemannFit::MatrixNd<n> matrixC_u(const riemannFit::VectorNd<n>& weights,
0216                                                                const riemannFit::VectorNd<n>& sTotal,
0217                                                                const riemannFit::VectorNd<n>& varBeta) {
0218     riemannFit::MatrixNd<n> c_uMat = riemannFit::MatrixNd<n>::Zero();
0219     for (u_int i = 0; i < n; i++) {
0220       c_uMat(i, i) = weights(i);
0221       if (i > 1)
0222         c_uMat(i, i) += 1. / (varBeta(i - 1) * riemannFit::sqr(sTotal(i) - sTotal(i - 1)));
0223       if (i > 0 && i < n - 1)
0224         c_uMat(i, i) +=
0225             (1. / varBeta(i)) * riemannFit::sqr((sTotal(i + 1) - sTotal(i - 1)) /
0226                                                 ((sTotal(i + 1) - sTotal(i)) * (sTotal(i) - sTotal(i - 1))));
0227       if (i < n - 2)
0228         c_uMat(i, i) += 1. / (varBeta(i + 1) * riemannFit::sqr(sTotal(i + 1) - sTotal(i)));
0229
0230       if (i > 0 && i < n - 1)
0231         c_uMat(i, i + 1) =
0232             1. / (varBeta(i) * (sTotal(i + 1) - sTotal(i))) *
0233             (-(sTotal(i + 1) - sTotal(i - 1)) / ((sTotal(i + 1) - sTotal(i)) * (sTotal(i) - sTotal(i - 1))));
0234       if (i < n - 2)
0235         c_uMat(i, i + 1) +=
0236             1. / (varBeta(i + 1) * (sTotal(i + 1) - sTotal(i))) *
0237             (-(sTotal(i + 2) - sTotal(i)) / ((sTotal(i + 2) - sTotal(i + 1)) * (sTotal(i + 1) - sTotal(i))));
0238
0239       if (i < n - 2)
0240         c_uMat(i, i + 2) = 1. / (varBeta(i + 1) * (sTotal(i + 2) - sTotal(i + 1)) * (sTotal(i + 1) - sTotal(i)));
0241
0242       c_uMat(i, i) *= 0.5;
0243     }
0244     return c_uMat + c_uMat.transpose();
0245   }
0246
0247   /*!
0248     \brief A very fast helix fit.
0249
0250     \param hits the measured hits.
0251
0252     \return (X0,Y0,R,tan(theta)).
0253
0254     \warning sign of theta is (intentionally, for now) mistaken for negative charges.
0255   */
0256
0257   template <typename M3xN, typename V4>
0258   __host__ __device__ inline void fastFit(const M3xN& hits, V4& result) {
0259     constexpr uint32_t n = M3xN::ColsAtCompileTime;
0260
0261     int mId = 1;
0262
0263     if constexpr (n > 3) {
0264       riemannFit::Vector2d middle = 0.5 * (hits.block(0, n - 1, 2, 1) + hits.block(0, 0, 2, 1));
0265       auto d1 = (hits.block(0, n / 2, 2, 1) - middle).squaredNorm();
0266       auto d2 = (hits.block(0, n / 2 - 1, 2, 1) - middle).squaredNorm();
0267       mId = d1 < d2 ? n / 2 : n / 2 - 1;
0268     }
0269
0270     const riemannFit::Vector2d a = hits.block(0, mId, 2, 1) - hits.block(0, 0, 2, 1);
0271     const riemannFit::Vector2d b = hits.block(0, n - 1, 2, 1) - hits.block(0, mId, 2, 1);
0272     const riemannFit::Vector2d c = hits.block(0, 0, 2, 1) - hits.block(0, n - 1, 2, 1);
0273
0274     auto tmp = 0.5 / riemannFit::cross2D(c, a);
0275     result(0) = hits(0, 0) - (a(1) * c.squaredNorm() + c(1) * a.squaredNorm()) * tmp;
0276     result(1) = hits(1, 0) + (a(0) * c.squaredNorm() + c(0) * a.squaredNorm()) * tmp;
0277     // check Wikipedia for these formulas
0278
0279     result(2) = sqrt(a.squaredNorm() * b.squaredNorm() * c.squaredNorm()) / (2. * std::abs(riemannFit::cross2D(b, a)));
0280     // Using Math Olympiad's formula R=abc/(4A)
0281
0282     const riemannFit::Vector2d d = hits.block(0, 0, 2, 1) - result.head(2);
0283     const riemannFit::Vector2d e = hits.block(0, n - 1, 2, 1) - result.head(2);
0284
0285     result(3) = result(2) * atan2(riemannFit::cross2D(d, e), d.dot(e)) / (hits(2, n - 1) - hits(2, 0));
0286     // ds/dz slope between last and first point
0287   }
0288
0289   /*!
0290     \brief Performs the Broken Line fit in the curved track case (that is, the fit
0291    *       parameters are the interceptions u and the curvature correction \Delta\kappa).
0292
0293     \param hits hits coordinates.
0294     \param hits_cov hits covariance matrix.
0295     \param fast_fit pre-fit result in the form (X0,Y0,R,tan(theta)).
0296     \param bField magnetic field in Gev/cm/c.
0297     \param data PreparedBrokenLineData.
0298     \param circle_results struct to be filled with the results in this form:
0299     -par parameter of the line in this form: (phi, d, k); \n
0300     -cov covariance matrix of the fitted parameter; \n
0301     -chi2 value of the cost function in the minimum.
0302
0303     \details The function implements the steps 2 and 3 of the Broken Line fit
0304    *         with the curvature correction.\n
0305    * The step 2 is the least square fit, done by imposing the minimum constraint on
0306    * the cost function and solving the consequent linear system. It determines the
0307    * fitted parameters u and \Delta\kappa and their covariance matrix.
0308    * The step 3 is the correction of the fast pre-fitted parameters for the innermost
0309    * part of the track. It is first done in a comfortable coordinate system (the one
0310    * in which the first hit is the origin) and then the parameters and their
0311    * covariance matrix are transformed to the original coordinate system.
0312   */
0313   template <typename M3xN, typename M6xN, typename V4, int n>
0314   __host__ __device__ inline void circleFit(const M3xN& hits,
0315                                             const M6xN& hits_ge,
0316                                             const V4& fast_fit,
0317                                             const double bField,
0318                                             PreparedBrokenLineData<n>& data,
0319                                             karimaki_circle_fit& circle_results) {
0320     circle_results.qCharge = data.qCharge;
0321     auto& radii = data.radii;
0322     const auto& sTransverse = data.sTransverse;
0323     const auto& sTotal = data.sTotal;
0324     auto& zInSZplane = data.zInSZplane;
0325     auto& varBeta = data.varBeta;
0326     const double slope = -circle_results.qCharge / fast_fit(3);
0327     varBeta *= 1. + riemannFit::sqr(slope);  // the kink angles are projected!
0328
0329     for (u_int i = 0; i < n; i++) {
0330       zInSZplane(i) = radii.block(0, i, 2, 1).norm() - fast_fit(2);
0331     }
0332
0333     riemannFit::Matrix2d vMat;           // covariance matrix
0334     riemannFit::VectorNd<n> weightsVec;  // weights
0335     riemannFit::Matrix2d rotMat;         // rotation matrix point by point
0336     for (u_int i = 0; i < n; i++) {
0337       vMat(0, 0) = hits_ge.col(i)[0];               // x errors
0338       vMat(0, 1) = vMat(1, 0) = hits_ge.col(i)[1];  // cov_xy
0339       vMat(1, 1) = hits_ge.col(i)[2];               // y errors
0340       rotMat = rotationMatrix(-radii(0, i) / radii(1, i));
0341       weightsVec(i) =
0342           1. / ((rotMat * vMat * rotMat.transpose())(1, 1));  // compute the orthogonal weight point by point
0343     }
0344
0345     riemannFit::VectorNplusONEd<n> r_uVec;
0346     r_uVec(n) = 0;
0347     for (u_int i = 0; i < n; i++) {
0348       r_uVec(i) = weightsVec(i) * zInSZplane(i);
0349     }
0350
0351     riemannFit::MatrixNplusONEd<n> c_uMat;
0352     c_uMat.block(0, 0, n, n) = matrixC_u(weightsVec, sTransverse, varBeta);
0353     c_uMat(n, n) = 0;
0354     //add the border to the c_uMat matrix
0355     for (u_int i = 0; i < n; i++) {
0356       c_uMat(i, n) = 0;
0357       if (i > 0 && i < n - 1) {
0358         c_uMat(i, n) +=
0359             -(sTransverse(i + 1) - sTransverse(i - 1)) * (sTransverse(i + 1) - sTransverse(i - 1)) /
0360             (2. * varBeta(i) * (sTransverse(i + 1) - sTransverse(i)) * (sTransverse(i) - sTransverse(i - 1)));
0361       }
0362       if (i > 1) {
0363         c_uMat(i, n) +=
0364             (sTransverse(i) - sTransverse(i - 2)) / (2. * varBeta(i - 1) * (sTransverse(i) - sTransverse(i - 1)));
0365       }
0366       if (i < n - 2) {
0367         c_uMat(i, n) +=
0368             (sTransverse(i + 2) - sTransverse(i)) / (2. * varBeta(i + 1) * (sTransverse(i + 1) - sTransverse(i)));
0369       }
0370       c_uMat(n, i) = c_uMat(i, n);
0371       if (i > 0 && i < n - 1)
0372         c_uMat(n, n) += riemannFit::sqr(sTransverse(i + 1) - sTransverse(i - 1)) / (4. * varBeta(i));
0373     }
0374
0375 #ifdef CPP_DUMP
0376     std::cout << "CU5\n" << c_uMat << std::endl;
0377 #endif
0378     riemannFit::MatrixNplusONEd<n> iMat;
0379     math::cholesky::invert(c_uMat, iMat);
0380 #ifdef CPP_DUMP
0381     std::cout << "I5\n" << iMat << std::endl;
0382 #endif
0383
0384     riemannFit::VectorNplusONEd<n> uVec = iMat * r_uVec;  // obtain the fitted parameters by solving the linear system
0385
0386     // compute (phi, d_ca, k) in the system in which the midpoint of the first two corrected hits is the origin...
0387
0388     radii.block(0, 0, 2, 1) /= radii.block(0, 0, 2, 1).norm();
0389     radii.block(0, 1, 2, 1) /= radii.block(0, 1, 2, 1).norm();
0390
0391     riemannFit::Vector2d dVec = hits.block(0, 0, 2, 1) + (-zInSZplane(0) + uVec(0)) * radii.block(0, 0, 2, 1);
0392     riemannFit::Vector2d eVec = hits.block(0, 1, 2, 1) + (-zInSZplane(1) + uVec(1)) * radii.block(0, 1, 2, 1);
0393     auto eMinusd = eVec - dVec;
0394     auto eMinusd2 = eMinusd.squaredNorm();
0395     auto tmp1 = 1. / eMinusd2;
0396     auto tmp2 = sqrt(riemannFit::sqr(fast_fit(2)) - 0.25 * eMinusd2);
0397
0398     circle_results.par << atan2(eMinusd(1), eMinusd(0)), circle_results.qCharge * (tmp2 - fast_fit(2)),
0399         circle_results.qCharge * (1. / fast_fit(2) + uVec(n));
0400
0401     tmp2 = 1. / tmp2;
0402
0403     riemannFit::Matrix3d jacobian;
0404     jacobian << (radii(1, 0) * eMinusd(0) - eMinusd(1) * radii(0, 0)) * tmp1,
0405         (radii(1, 1) * eMinusd(0) - eMinusd(1) * radii(0, 1)) * tmp1, 0,
0406         circle_results.qCharge * (eMinusd(0) * radii(0, 0) + eMinusd(1) * radii(1, 0)) * tmp2,
0407         circle_results.qCharge * (eMinusd(0) * radii(0, 1) + eMinusd(1) * radii(1, 1)) * tmp2, 0, 0, 0,
0408         circle_results.qCharge;
0409
0410     circle_results.cov << iMat(0, 0), iMat(0, 1), iMat(0, n), iMat(1, 0), iMat(1, 1), iMat(1, n), iMat(n, 0),
0411         iMat(n, 1), iMat(n, n);
0412
0413     circle_results.cov = jacobian * circle_results.cov * jacobian.transpose();
0414
0415     //...Translate in the system in which the first corrected hit is the origin, adding the m.s. correction...
0416
0417     translateKarimaki(circle_results, 0.5 * eMinusd(0), 0.5 * eMinusd(1), jacobian);
0418     circle_results.cov(0, 0) +=
0419         (1 + riemannFit::sqr(slope)) * multScatt(sTotal(1) - sTotal(0), bField, fast_fit(2), 2, slope);
0420
0421     //...And translate back to the original system
0422
0423     translateKarimaki(circle_results, dVec(0), dVec(1), jacobian);
0424
0425     // compute chi2
0426     circle_results.chi2 = 0;
0427     for (u_int i = 0; i < n; i++) {
0428       circle_results.chi2 += weightsVec(i) * riemannFit::sqr(zInSZplane(i) - uVec(i));
0429       if (i > 0 && i < n - 1)
0430         circle_results.chi2 +=
0431             riemannFit::sqr(uVec(i - 1) / (sTransverse(i) - sTransverse(i - 1)) -
0432                             uVec(i) * (sTransverse(i + 1) - sTransverse(i - 1)) /
0433                                 ((sTransverse(i + 1) - sTransverse(i)) * (sTransverse(i) - sTransverse(i - 1))) +
0434                             uVec(i + 1) / (sTransverse(i + 1) - sTransverse(i)) +
0435                             (sTransverse(i + 1) - sTransverse(i - 1)) * uVec(n) / 2) /
0436             varBeta(i);
0437     }
0438   }
0439
0440   /*!
0441     \brief Performs the Broken Line fit in the straight track case (that is, the fit parameters are only the interceptions u).
0442
0443     \param hits hits coordinates.
0444     \param fast_fit pre-fit result in the form (X0,Y0,R,tan(theta)).
0445     \param bField magnetic field in Gev/cm/c.
0446     \param data PreparedBrokenLineData.
0447     \param line_results struct to be filled with the results in this form:
0448     -par parameter of the line in this form: (cot(theta), Zip); \n
0449     -cov covariance matrix of the fitted parameter; \n
0450     -chi2 value of the cost function in the minimum.
0451
0452     \details The function implements the steps 2 and 3 of the Broken Line fit without
0453    *        the curvature correction.\n
0454    * The step 2 is the least square fit, done by imposing the minimum constraint
0455    * on the cost function and solving the consequent linear system. It determines
0456    * the fitted parameters u and their covariance matrix.
0457    * The step 3 is the correction of the fast pre-fitted parameters for the innermost
0458    * part of the track. It is first done in a comfortable coordinate system (the one
0459    * in which the first hit is the origin) and then the parameters and their covariance
0460    * matrix are transformed to the original coordinate system.
0461    */
0462   template <typename V4, typename M6xN, int n>
0463   __host__ __device__ inline void lineFit(const M6xN& hits_ge,
0464                                           const V4& fast_fit,
0465                                           const double bField,
0466                                           const PreparedBrokenLineData<n>& data,
0467                                           riemannFit::LineFit& line_results) {
0468     const auto& radii = data.radii;
0469     const auto& sTotal = data.sTotal;
0470     const auto& zInSZplane = data.zInSZplane;
0471     const auto& varBeta = data.varBeta;
0472
0473     const double slope = -data.qCharge / fast_fit(3);
0474     riemannFit::Matrix2d rotMat = rotationMatrix(slope);
0475
0476     riemannFit::Matrix3d vMat = riemannFit::Matrix3d::Zero();  // covariance matrix XYZ
0477     riemannFit::Matrix2x3d jacobXYZtosZ =
0478         riemannFit::Matrix2x3d::Zero();  // jacobian for computation of the error on s (xyz -> sz)
0479     riemannFit::VectorNd<n> weights = riemannFit::VectorNd<n>::Zero();
0480     for (u_int i = 0; i < n; i++) {
0481       vMat(0, 0) = hits_ge.col(i)[0];               // x errors
0482       vMat(0, 1) = vMat(1, 0) = hits_ge.col(i)[1];  // cov_xy
0483       vMat(0, 2) = vMat(2, 0) = hits_ge.col(i)[3];  // cov_xz
0484       vMat(1, 1) = hits_ge.col(i)[2];               // y errors
0485       vMat(2, 1) = vMat(1, 2) = hits_ge.col(i)[4];  // cov_yz
0486       vMat(2, 2) = hits_ge.col(i)[5];               // z errors
0487       auto tmp = 1. / radii.block(0, i, 2, 1).norm();
0488       jacobXYZtosZ(0, 0) = radii(1, i) * tmp;
0489       jacobXYZtosZ(0, 1) = -radii(0, i) * tmp;
0490       jacobXYZtosZ(1, 2) = 1.;
0491       weights(i) = 1. / ((rotMat * jacobXYZtosZ * vMat * jacobXYZtosZ.transpose() * rotMat.transpose())(
0492                             1, 1));  // compute the orthogonal weight point by point
0493     }
0494
0495     riemannFit::VectorNd<n> r_u;
0496     for (u_int i = 0; i < n; i++) {
0497       r_u(i) = weights(i) * zInSZplane(i);
0498     }
0499 #ifdef CPP_DUMP
0500     std::cout << "CU4\n" << matrixC_u(w, sTotal, varBeta) << std::endl;
0501 #endif
0502     riemannFit::MatrixNd<n> iMat;
0503     math::cholesky::invert(matrixC_u(weights, sTotal, varBeta), iMat);
0504 #ifdef CPP_DUMP
0505     std::cout << "I4\n" << iMat << std::endl;
0506 #endif
0507
0508     riemannFit::VectorNd<n> uVec = iMat * r_u;  // obtain the fitted parameters by solving the linear system
0509
0510     // line parameters in the system in which the first hit is the origin and with axis along SZ
0511     line_results.par << (uVec(1) - uVec(0)) / (sTotal(1) - sTotal(0)), uVec(0);
0512     auto idiff = 1. / (sTotal(1) - sTotal(0));
0513     line_results.cov << (iMat(0, 0) - 2 * iMat(0, 1) + iMat(1, 1)) * riemannFit::sqr(idiff) +
0514                             multScatt(sTotal(1) - sTotal(0), bField, fast_fit(2), 2, slope),
0515         (iMat(0, 1) - iMat(0, 0)) * idiff, (iMat(0, 1) - iMat(0, 0)) * idiff, iMat(0, 0);
0516
0517     // translate to the original SZ system
0518     riemannFit::Matrix2d jacobian;
0519     jacobian(0, 0) = 1.;
0520     jacobian(0, 1) = 0;
0521     jacobian(1, 0) = -sTotal(0);
0522     jacobian(1, 1) = 1.;
0523     line_results.par(1) += -line_results.par(0) * sTotal(0);
0524     line_results.cov = jacobian * line_results.cov * jacobian.transpose();
0525
0526     // rotate to the original sz system
0527     auto tmp = rotMat(0, 0) - line_results.par(0) * rotMat(0, 1);
0528     jacobian(1, 1) = 1. / tmp;
0529     jacobian(0, 0) = jacobian(1, 1) * jacobian(1, 1);
0530     jacobian(0, 1) = 0;
0531     jacobian(1, 0) = line_results.par(1) * rotMat(0, 1) * jacobian(0, 0);
0532     line_results.par(1) = line_results.par(1) * jacobian(1, 1);
0533     line_results.par(0) = (rotMat(0, 1) + line_results.par(0) * rotMat(0, 0)) * jacobian(1, 1);
0534     line_results.cov = jacobian * line_results.cov * jacobian.transpose();
0535
0536     // compute chi2
0537     line_results.chi2 = 0;
0538     for (u_int i = 0; i < n; i++) {
0539       line_results.chi2 += weights(i) * riemannFit::sqr(zInSZplane(i) - uVec(i));
0540       if (i > 0 && i < n - 1)
0541         line_results.chi2 += riemannFit::sqr(uVec(i - 1) / (sTotal(i) - sTotal(i - 1)) -
0542                                              uVec(i) * (sTotal(i + 1) - sTotal(i - 1)) /
0543                                                  ((sTotal(i + 1) - sTotal(i)) * (sTotal(i) - sTotal(i - 1))) +
0544                                              uVec(i + 1) / (sTotal(i + 1) - sTotal(i))) /
0545                              varBeta(i);
0546     }
0547   }
0548
0549   /*!
0550     \brief Helix fit by three step:
0551     -fast pre-fit (see Fast_fit() for further info); \n
0552     -circle fit of the hits projected in the transverse plane by Broken Line algorithm (see BL_Circle_fit() for further info); \n
0553     -line fit of the hits projected on the (pre-fitted) cilinder surface by Broken Line algorithm (see BL_Line_fit() for further info); \n
0554     Points must be passed ordered (from inner to outer layer).
0555
0556     \param hits Matrix3xNd hits coordinates in this form: \n
0557     |x1|x2|x3|...|xn| \n
0558     |y1|y2|y3|...|yn| \n
0559     |z1|z2|z3|...|zn|
0560     \param hits_cov Matrix3Nd covariance matrix in this form (()->cov()): \n
0561     |(x1,x1)|(x2,x1)|(x3,x1)|(x4,x1)|.|(y1,x1)|(y2,x1)|(y3,x1)|(y4,x1)|.|(z1,x1)|(z2,x1)|(z3,x1)|(z4,x1)| \n
0562     |(x1,x2)|(x2,x2)|(x3,x2)|(x4,x2)|.|(y1,x2)|(y2,x2)|(y3,x2)|(y4,x2)|.|(z1,x2)|(z2,x2)|(z3,x2)|(z4,x2)| \n
0563     |(x1,x3)|(x2,x3)|(x3,x3)|(x4,x3)|.|(y1,x3)|(y2,x3)|(y3,x3)|(y4,x3)|.|(z1,x3)|(z2,x3)|(z3,x3)|(z4,x3)| \n
0564     |(x1,x4)|(x2,x4)|(x3,x4)|(x4,x4)|.|(y1,x4)|(y2,x4)|(y3,x4)|(y4,x4)|.|(z1,x4)|(z2,x4)|(z3,x4)|(z4,x4)| \n
0565     .       .       .       .       . .       .       .       .       . .       .       .       .       . \n
0566     |(x1,y1)|(x2,y1)|(x3,y1)|(x4,y1)|.|(y1,y1)|(y2,y1)|(y3,x1)|(y4,y1)|.|(z1,y1)|(z2,y1)|(z3,y1)|(z4,y1)| \n
0567     |(x1,y2)|(x2,y2)|(x3,y2)|(x4,y2)|.|(y1,y2)|(y2,y2)|(y3,x2)|(y4,y2)|.|(z1,y2)|(z2,y2)|(z3,y2)|(z4,y2)| \n
0568     |(x1,y3)|(x2,y3)|(x3,y3)|(x4,y3)|.|(y1,y3)|(y2,y3)|(y3,x3)|(y4,y3)|.|(z1,y3)|(z2,y3)|(z3,y3)|(z4,y3)| \n
0569     |(x1,y4)|(x2,y4)|(x3,y4)|(x4,y4)|.|(y1,y4)|(y2,y4)|(y3,x4)|(y4,y4)|.|(z1,y4)|(z2,y4)|(z3,y4)|(z4,y4)| \n
0570     .       .       .    .          . .       .       .       .       . .       .       .       .       . \n
0571     |(x1,z1)|(x2,z1)|(x3,z1)|(x4,z1)|.|(y1,z1)|(y2,z1)|(y3,z1)|(y4,z1)|.|(z1,z1)|(z2,z1)|(z3,z1)|(z4,z1)| \n
0572     |(x1,z2)|(x2,z2)|(x3,z2)|(x4,z2)|.|(y1,z2)|(y2,z2)|(y3,z2)|(y4,z2)|.|(z1,z2)|(z2,z2)|(z3,z2)|(z4,z2)| \n
0573     |(x1,z3)|(x2,z3)|(x3,z3)|(x4,z3)|.|(y1,z3)|(y2,z3)|(y3,z3)|(y4,z3)|.|(z1,z3)|(z2,z3)|(z3,z3)|(z4,z3)| \n
0574     |(x1,z4)|(x2,z4)|(x3,z4)|(x4,z4)|.|(y1,z4)|(y2,z4)|(y3,z4)|(y4,z4)|.|(z1,z4)|(z2,z4)|(z3,z4)|(z4,z4)|
0575     \param bField magnetic field in the center of the detector in Gev/cm/c, in order to perform the p_t calculation.
0576
0577     \warning see BL_Circle_fit(), BL_Line_fit() and Fast_fit() warnings.
0578
0579     \bug see BL_Circle_fit(), BL_Line_fit() and Fast_fit() bugs.
0580
0581     \return (phi,Tip,p_t,cot(theta)),Zip), their covariance matrix and the chi2's of the circle and line fits.
0582   */
0583   template <int n>
0584   inline riemannFit::HelixFit helixFit(const riemannFit::Matrix3xNd<n>& hits,
0585                                        const Eigen::Matrix<float, 6, 4>& hits_ge,
0586                                        const double bField) {
0587     riemannFit::HelixFit helix;
0588     riemannFit::Vector4d fast_fit;
0589     fastFit(hits, fast_fit);
0590
0591     PreparedBrokenLineData<n> data;
0592     karimaki_circle_fit circle;
0593     riemannFit::LineFit line;
0594     riemannFit::Matrix3d jacobian;
0595
0596     prepareBrokenLineData(hits, fast_fit, bField, data);
0597     lineFit(hits_ge, fast_fit, bField, data, line);
0598     circleFit(hits, hits_ge, fast_fit, bField, data, circle);
0599
0600     // the circle fit gives k, but here we want p_t, so let's change the parameter and the covariance matrix
0601     jacobian << 1., 0, 0, 0, 1., 0, 0, 0,
0602         -std::abs(circle.par(2)) * bField / (riemannFit::sqr(circle.par(2)) * circle.par(2));
0603     circle.par(2) = bField / std::abs(circle.par(2));
0604     circle.cov = jacobian * circle.cov * jacobian.transpose();
0605
0606     helix.par << circle.par, line.par;
0607     helix.cov = riemannFit::MatrixXd::Zero(5, 5);
0608     helix.cov.block(0, 0, 3, 3) = circle.cov;
0609     helix.cov.block(3, 3, 2, 2) = line.cov;
0610     helix.qCharge = circle.qCharge;
0611     helix.chi2_circle = circle.chi2;
0612     helix.chi2_line = line.chi2;
0613
0614     return helix;
0615   }
0616
0617 }  // namespace brokenline
0618
0619 #endif  // RecoTracker_PixelTrackFitting_BrokenLine_h