PixelTrackFitting/interface/BrokenLine.h

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