Back to home page

Project CMSSW displayed by LXR

 
 

    


File indexing completed on 2021-06-03 02:42:04

0001 #ifndef RecoPixelVertexing_PixelTrackFitting_interface_RiemannFit_h
0002 #define RecoPixelVertexing_PixelTrackFitting_interface_RiemannFit_h
0003 
0004 #include "RecoPixelVertexing/PixelTrackFitting/interface/FitUtils.h"
0005 
0006 namespace riemannFit {
0007 
0008   /*!  Compute the Radiation length in the uniform hypothesis
0009  *
0010  * The Pixel detector, barrel and forward, is considered as an homogeneous
0011  * cylinder of material, whose radiation lengths has been derived from the TDR
0012  * plot that shows that 16cm correspond to 0.06 radiation lengths. Therefore
0013  * one radiation length corresponds to 16cm/0.06 =~ 267 cm. All radiation
0014  * lengths are computed using this unique number, in both regions, barrel and
0015  * endcap.
0016  *
0017  * NB: no angle corrections nor projections are computed inside this routine.
0018  * It is therefore the responsibility of the caller to supply the proper
0019  * lengths in input. These lengths are the path traveled by the particle along
0020  * its trajectory, namely the so called S of the helix in 3D space.
0021  *
0022  * \param length_values vector of incremental distances that will be translated
0023  * into radiation length equivalent. Each radiation length i is computed
0024  * incrementally with respect to the previous length i-1. The first length has
0025  * no reference point (i.e. it has the dca).
0026  *
0027  * \return incremental radiation lengths that correspond to each segment.
0028  */
0029 
0030   template <typename VNd1, typename VNd2>
0031   __host__ __device__ inline void computeRadLenUniformMaterial(const VNd1& length_values, VNd2& rad_lengths) {
0032     // Radiation length of the pixel detector in the uniform assumption, with
0033     // 0.06 rad_len at 16 cm
0034     constexpr double xx_0_inv = 0.06 / 16.;
0035     uint n = length_values.rows();
0036     rad_lengths(0) = length_values(0) * xx_0_inv;
0037     for (uint j = 1; j < n; ++j) {
0038       rad_lengths(j) = std::abs(length_values(j) - length_values(j - 1)) * xx_0_inv;
0039     }
0040   }
0041 
0042   /*!
0043     \brief Compute the covariance matrix along cartesian S-Z of points due to
0044     multiple Coulomb scattering to be used in the line_fit, for the barrel
0045     and forward cases.
0046     The input covariance matrix is in the variables s-z, original and
0047     unrotated.
0048     The multiple scattering component is computed in the usual linear
0049     approximation, using the 3D path which is computed as the squared root of
0050     the squared sum of the s and z components passed in.
0051     Internally a rotation by theta is performed and the covariance matrix
0052     returned is the one in the direction orthogonal to the rotated S3D axis,
0053     i.e. along the rotated Z axis.
0054     The choice of the rotation is not arbitrary, but derived from the fact that
0055     putting the horizontal axis along the S3D direction allows the usage of the
0056     ordinary least squared fitting techiques with the trivial parametrization y
0057     = mx + q, avoiding the patological case with m = +/- inf, that would
0058     correspond to the case at eta = 0.
0059  */
0060 
0061   template <typename V4, typename VNd1, typename VNd2, int N>
0062   __host__ __device__ inline auto scatterCovLine(Matrix2d const* cov_sz,
0063                                                  const V4& fast_fit,
0064                                                  VNd1 const& s_arcs,
0065                                                  VNd2 const& z_values,
0066                                                  const double theta,
0067                                                  const double bField,
0068                                                  MatrixNd<N>& ret) {
0069 #ifdef RFIT_DEBUG
0070     riemannFit::printIt(&s_arcs, "Scatter_cov_line - s_arcs: ");
0071 #endif
0072     constexpr uint n = N;
0073     double p_t = std::min(20., fast_fit(2) * bField);  // limit pt to avoid too small error!!!
0074     double p_2 = p_t * p_t * (1. + 1. / sqr(fast_fit(3)));
0075     VectorNd<N> rad_lengths_S;
0076     // See documentation at http://eigen.tuxfamily.org/dox/group__TutorialArrayClass.html
0077     // Basically, to perform cwise operations on Matrices and Vectors, you need
0078     // to transform them into Array-like objects.
0079     VectorNd<N> s_values = s_arcs.array() * s_arcs.array() + z_values.array() * z_values.array();
0080     s_values = s_values.array().sqrt();
0081     computeRadLenUniformMaterial(s_values, rad_lengths_S);
0082     VectorNd<N> sig2_S;
0083     sig2_S = .000225 / p_2 * (1. + 0.038 * rad_lengths_S.array().log()).abs2() * rad_lengths_S.array();
0084 #ifdef RFIT_DEBUG
0085     riemannFit::printIt(cov_sz, "Scatter_cov_line - cov_sz: ");
0086 #endif
0087     Matrix2Nd<N> tmp = Matrix2Nd<N>::Zero();
0088     for (uint k = 0; k < n; ++k) {
0089       tmp(k, k) = cov_sz[k](0, 0);
0090       tmp(k + n, k + n) = cov_sz[k](1, 1);
0091       tmp(k, k + n) = tmp(k + n, k) = cov_sz[k](0, 1);
0092     }
0093     for (uint k = 0; k < n; ++k) {
0094       for (uint l = k; l < n; ++l) {
0095         for (uint i = 0; i < std::min(k, l); ++i) {
0096           tmp(k + n, l + n) += std::abs(s_values(k) - s_values(i)) * std::abs(s_values(l) - s_values(i)) * sig2_S(i);
0097         }
0098         tmp(l + n, k + n) = tmp(k + n, l + n);
0099       }
0100     }
0101     // We are interested only in the errors orthogonal to the rotated s-axis
0102     // which, in our formalism, are in the lower square matrix.
0103 #ifdef RFIT_DEBUG
0104     riemannFit::printIt(&tmp, "Scatter_cov_line - tmp: ");
0105 #endif
0106     ret = tmp.block(n, n, n, n);
0107   }
0108 
0109   /*!
0110     \brief Compute the covariance matrix (in radial coordinates) of points in
0111     the transverse plane due to multiple Coulomb scattering.
0112     \param p2D 2D points in the transverse plane.
0113     \param fast_fit fast_fit Vector4d result of the previous pre-fit
0114     structured in this form:(X0, Y0, R, Tan(Theta))).
0115     \param B magnetic field use to compute p
0116     \return scatter_cov_rad errors due to multiple scattering.
0117     \warning input points must be ordered radially from the detector center
0118     (from inner layer to outer ones; points on the same layer must ordered too).
0119     \details Only the tangential component is computed (the radial one is
0120     negligible).
0121  */
0122   template <typename M2xN, typename V4, int N>
0123   __host__ __device__ inline MatrixNd<N> scatter_cov_rad(const M2xN& p2D,
0124                                                          const V4& fast_fit,
0125                                                          VectorNd<N> const& rad,
0126                                                          double B) {
0127     constexpr uint n = N;
0128     double p_t = std::min(20., fast_fit(2) * B);  // limit pt to avoid too small error!!!
0129     double p_2 = p_t * p_t * (1. + 1. / sqr(fast_fit(3)));
0130     double theta = atan(fast_fit(3));
0131     theta = theta < 0. ? theta + M_PI : theta;
0132     VectorNd<N> s_values;
0133     VectorNd<N> rad_lengths;
0134     const Vector2d oVec(fast_fit(0), fast_fit(1));
0135 
0136     // associated Jacobian, used in weights and errors computation
0137     for (uint i = 0; i < n; ++i) {  // x
0138       Vector2d pVec = p2D.block(0, i, 2, 1) - oVec;
0139       const double cross = cross2D(-oVec, pVec);
0140       const double dot = (-oVec).dot(pVec);
0141       const double tempAtan2 = atan2(cross, dot);
0142       s_values(i) = std::abs(tempAtan2 * fast_fit(2));
0143     }
0144     computeRadLenUniformMaterial(s_values * sqrt(1. + 1. / sqr(fast_fit(3))), rad_lengths);
0145     MatrixNd<N> scatter_cov_rad = MatrixNd<N>::Zero();
0146     VectorNd<N> sig2 = (1. + 0.038 * rad_lengths.array().log()).abs2() * rad_lengths.array();
0147     sig2 *= 0.000225 / (p_2 * sqr(sin(theta)));
0148     for (uint k = 0; k < n; ++k) {
0149       for (uint l = k; l < n; ++l) {
0150         for (uint i = 0; i < std::min(k, l); ++i) {
0151           scatter_cov_rad(k, l) += (rad(k) - rad(i)) * (rad(l) - rad(i)) * sig2(i);
0152         }
0153         scatter_cov_rad(l, k) = scatter_cov_rad(k, l);
0154       }
0155     }
0156 #ifdef RFIT_DEBUG
0157     riemannFit::printIt(&scatter_cov_rad, "Scatter_cov_rad - scatter_cov_rad: ");
0158 #endif
0159     return scatter_cov_rad;
0160   }
0161 
0162   /*!
0163     \brief Transform covariance matrix from radial (only tangential component)
0164     to Cartesian coordinates (only transverse plane component).
0165     \param p2D 2D points in the transverse plane.
0166     \param cov_rad covariance matrix in radial coordinate.
0167     \return cov_cart covariance matrix in Cartesian coordinates.
0168 */
0169 
0170   template <typename M2xN, int N>
0171   __host__ __device__ inline Matrix2Nd<N> cov_radtocart(const M2xN& p2D,
0172                                                         const MatrixNd<N>& cov_rad,
0173                                                         const VectorNd<N>& rad) {
0174 #ifdef RFIT_DEBUG
0175     printf("Address of p2D: %p\n", &p2D);
0176 #endif
0177     printIt(&p2D, "cov_radtocart - p2D:");
0178     constexpr uint n = N;
0179     Matrix2Nd<N> cov_cart = Matrix2Nd<N>::Zero();
0180     VectorNd<N> rad_inv = rad.cwiseInverse();
0181     printIt(&rad_inv, "cov_radtocart - rad_inv:");
0182     for (uint i = 0; i < n; ++i) {
0183       for (uint j = i; j < n; ++j) {
0184         cov_cart(i, j) = cov_rad(i, j) * p2D(1, i) * rad_inv(i) * p2D(1, j) * rad_inv(j);
0185         cov_cart(i + n, j + n) = cov_rad(i, j) * p2D(0, i) * rad_inv(i) * p2D(0, j) * rad_inv(j);
0186         cov_cart(i, j + n) = -cov_rad(i, j) * p2D(1, i) * rad_inv(i) * p2D(0, j) * rad_inv(j);
0187         cov_cart(i + n, j) = -cov_rad(i, j) * p2D(0, i) * rad_inv(i) * p2D(1, j) * rad_inv(j);
0188         cov_cart(j, i) = cov_cart(i, j);
0189         cov_cart(j + n, i + n) = cov_cart(i + n, j + n);
0190         cov_cart(j + n, i) = cov_cart(i, j + n);
0191         cov_cart(j, i + n) = cov_cart(i + n, j);
0192       }
0193     }
0194     return cov_cart;
0195   }
0196 
0197   /*!
0198     \brief Transform covariance matrix from Cartesian coordinates (only
0199     transverse plane component) to radial coordinates (both radial and
0200     tangential component but only diagonal terms, correlation between different
0201     point are not managed).
0202     \param p2D 2D points in transverse plane.
0203     \param cov_cart covariance matrix in Cartesian coordinates.
0204     \return cov_rad covariance matrix in raidal coordinate.
0205     \warning correlation between different point are not computed.
0206 */
0207   template <typename M2xN, int N>
0208   __host__ __device__ inline VectorNd<N> cov_carttorad(const M2xN& p2D,
0209                                                        const Matrix2Nd<N>& cov_cart,
0210                                                        const VectorNd<N>& rad) {
0211     constexpr uint n = N;
0212     VectorNd<N> cov_rad;
0213     const VectorNd<N> rad_inv2 = rad.cwiseInverse().array().square();
0214     for (uint i = 0; i < n; ++i) {
0215       //!< in case you have (0,0) to avoid dividing by 0 radius
0216       if (rad(i) < 1.e-4)
0217         cov_rad(i) = cov_cart(i, i);
0218       else {
0219         cov_rad(i) = rad_inv2(i) * (cov_cart(i, i) * sqr(p2D(1, i)) + cov_cart(i + n, i + n) * sqr(p2D(0, i)) -
0220                                     2. * cov_cart(i, i + n) * p2D(0, i) * p2D(1, i));
0221       }
0222     }
0223     return cov_rad;
0224   }
0225 
0226   /*!
0227     \brief Transform covariance matrix from Cartesian coordinates (only
0228     transverse plane component) to coordinates system orthogonal to the
0229     pre-fitted circle in each point.
0230     Further information in attached documentation.
0231     \param p2D 2D points in transverse plane.
0232     \param cov_cart covariance matrix in Cartesian coordinates.
0233     \param fast_fit fast_fit Vector4d result of the previous pre-fit
0234     structured in this form:(X0, Y0, R, tan(theta))).
0235     \return cov_rad covariance matrix in the pre-fitted circle's
0236     orthogonal system.
0237 */
0238   template <typename M2xN, typename V4, int N>
0239   __host__ __device__ inline VectorNd<N> cov_carttorad_prefit(const M2xN& p2D,
0240                                                               const Matrix2Nd<N>& cov_cart,
0241                                                               V4& fast_fit,
0242                                                               const VectorNd<N>& rad) {
0243     constexpr uint n = N;
0244     VectorNd<N> cov_rad;
0245     for (uint i = 0; i < n; ++i) {
0246       //!< in case you have (0,0) to avoid dividing by 0 radius
0247       if (rad(i) < 1.e-4)
0248         cov_rad(i) = cov_cart(i, i);  // TO FIX
0249       else {
0250         Vector2d a = p2D.col(i);
0251         Vector2d b = p2D.col(i) - fast_fit.head(2);
0252         const double x2 = a.dot(b);
0253         const double y2 = cross2D(a, b);
0254         const double tan_c = -y2 / x2;
0255         const double tan_c2 = sqr(tan_c);
0256         cov_rad(i) =
0257             1. / (1. + tan_c2) * (cov_cart(i, i) + cov_cart(i + n, i + n) * tan_c2 + 2 * cov_cart(i, i + n) * tan_c);
0258       }
0259     }
0260     return cov_rad;
0261   }
0262 
0263   /*!
0264     \brief Compute the points' weights' vector for the circle fit when multiple
0265     scattering is managed.
0266     Further information in attached documentation.
0267     \param cov_rad_inv covariance matrix inverse in radial coordinated
0268     (or, beter, pre-fitted circle's orthogonal system).
0269     \return weight VectorNd points' weights' vector.
0270     \bug I'm not sure this is the right way to compute the weights for non
0271     diagonal cov matrix. Further investigation needed.
0272 */
0273 
0274   template <int N>
0275   __host__ __device__ inline VectorNd<N> weightCircle(const MatrixNd<N>& cov_rad_inv) {
0276     return cov_rad_inv.colwise().sum().transpose();
0277   }
0278 
0279   /*!
0280     \brief Find particle q considering the  sign of cross product between
0281     particles velocity (estimated by the first 2 hits) and the vector radius
0282     between the first hit and the center of the fitted circle.
0283     \param p2D 2D points in transverse plane.
0284     \param par_uvr result of the circle fit in this form: (X0,Y0,R).
0285     \return q int 1 or -1.
0286 */
0287   template <typename M2xN>
0288   __host__ __device__ inline int32_t charge(const M2xN& p2D, const Vector3d& par_uvr) {
0289     return ((p2D(0, 1) - p2D(0, 0)) * (par_uvr.y() - p2D(1, 0)) - (p2D(1, 1) - p2D(1, 0)) * (par_uvr.x() - p2D(0, 0)) >
0290             0)
0291                ? -1
0292                : 1;
0293   }
0294 
0295   /*!
0296     \brief Compute the eigenvector associated to the minimum eigenvalue.
0297     \param A the Matrix you want to know eigenvector and eigenvalue.
0298     \param chi2 the double were the chi2-related quantity will be stored.
0299     \return the eigenvector associated to the minimum eigenvalue.
0300     \warning double precision is needed for a correct assessment of chi2.
0301     \details The minimus eigenvalue is related to chi2.
0302     We exploit the fact that the matrix is symmetrical and small (2x2 for line
0303     fit and 3x3 for circle fit), so the SelfAdjointEigenSolver from Eigen
0304     library is used, with the computedDirect  method (available only for 2x2
0305     and 3x3 Matrix) wich computes eigendecomposition of given matrix using a
0306     fast closed-form algorithm.
0307     For this optimization the matrix type must be known at compiling time.
0308 */
0309 
0310   __host__ __device__ inline Vector3d min_eigen3D(const Matrix3d& A, double& chi2) {
0311 #ifdef RFIT_DEBUG
0312     printf("min_eigen3D - enter\n");
0313 #endif
0314     Eigen::SelfAdjointEigenSolver<Matrix3d> solver(3);
0315     solver.computeDirect(A);
0316     int min_index;
0317     chi2 = solver.eigenvalues().minCoeff(&min_index);
0318 #ifdef RFIT_DEBUG
0319     printf("min_eigen3D - exit\n");
0320 #endif
0321     return solver.eigenvectors().col(min_index);
0322   }
0323 
0324   /*!
0325     \brief A faster version of min_eigen3D() where double precision is not
0326     needed.
0327     \param A the Matrix you want to know eigenvector and eigenvalue.
0328     \param chi2 the double were the chi2-related quantity will be stored
0329     \return the eigenvector associated to the minimum eigenvalue.
0330     \detail The computedDirect() method of SelfAdjointEigenSolver for 3x3 Matrix
0331     indeed, use trigonometry function (it solves a third degree equation) which
0332     speed up in  single precision.
0333 */
0334 
0335   __host__ __device__ inline Vector3d min_eigen3D_fast(const Matrix3d& A) {
0336     Eigen::SelfAdjointEigenSolver<Matrix3f> solver(3);
0337     solver.computeDirect(A.cast<float>());
0338     int min_index;
0339     solver.eigenvalues().minCoeff(&min_index);
0340     return solver.eigenvectors().col(min_index).cast<double>();
0341   }
0342 
0343   /*!
0344     \brief 2D version of min_eigen3D().
0345     \param aMat the Matrix you want to know eigenvector and eigenvalue.
0346     \param chi2 the double were the chi2-related quantity will be stored
0347     \return the eigenvector associated to the minimum eigenvalue.
0348     \detail The computedDirect() method of SelfAdjointEigenSolver for 2x2 Matrix
0349     do not use special math function (just sqrt) therefore it doesn't speed up
0350     significantly in single precision.
0351 */
0352 
0353   __host__ __device__ inline Vector2d min_eigen2D(const Matrix2d& aMat, double& chi2) {
0354     Eigen::SelfAdjointEigenSolver<Matrix2d> solver(2);
0355     solver.computeDirect(aMat);
0356     int min_index;
0357     chi2 = solver.eigenvalues().minCoeff(&min_index);
0358     return solver.eigenvectors().col(min_index);
0359   }
0360 
0361   /*!
0362     \brief A very fast helix fit: it fits a circle by three points (first, middle
0363     and last point) and a line by two points (first and last).
0364     \param hits points to be fitted
0365     \return result in this form: (X0,Y0,R,tan(theta)).
0366     \warning points must be passed ordered (from internal layer to external) in
0367     order to maximize accuracy and do not mistake tan(theta) sign.
0368     \details This fast fit is used as pre-fit which is needed for:
0369     - weights estimation and chi2 computation in line fit (fundamental);
0370     - weights estimation and chi2 computation in circle fit (useful);
0371     - computation of error due to multiple scattering.
0372 */
0373 
0374   template <typename M3xN, typename V4>
0375   __host__ __device__ inline void fastFit(const M3xN& hits, V4& result) {
0376     constexpr uint32_t N = M3xN::ColsAtCompileTime;
0377     constexpr auto n = N;  // get the number of hits
0378     printIt(&hits, "Fast_fit - hits: ");
0379 
0380     // CIRCLE FIT
0381     // Make segments between middle-to-first(b) and last-to-first(c) hits
0382     const Vector2d bVec = hits.block(0, n / 2, 2, 1) - hits.block(0, 0, 2, 1);
0383     const Vector2d cVec = hits.block(0, n - 1, 2, 1) - hits.block(0, 0, 2, 1);
0384     printIt(&bVec, "Fast_fit - b: ");
0385     printIt(&cVec, "Fast_fit - c: ");
0386     // Compute their lengths
0387     auto b2 = bVec.squaredNorm();
0388     auto c2 = cVec.squaredNorm();
0389     // The algebra has been verified (MR). The usual approach has been followed:
0390     // * use an orthogonal reference frame passing from the first point.
0391     // * build the segments (chords)
0392     // * build orthogonal lines through mid points
0393     // * make a system and solve for X0 and Y0.
0394     // * add the initial point
0395     bool flip = abs(bVec.x()) < abs(bVec.y());
0396     auto bx = flip ? bVec.y() : bVec.x();
0397     auto by = flip ? bVec.x() : bVec.y();
0398     auto cx = flip ? cVec.y() : cVec.x();
0399     auto cy = flip ? cVec.x() : cVec.y();
0400     //!< in case b.x is 0 (2 hits with same x)
0401     auto div = 2. * (cx * by - bx * cy);
0402     // if aligned TO FIX
0403     auto y0 = (cx * b2 - bx * c2) / div;
0404     auto x0 = (0.5 * b2 - y0 * by) / bx;
0405     result(0) = hits(0, 0) + (flip ? y0 : x0);
0406     result(1) = hits(1, 0) + (flip ? x0 : y0);
0407     result(2) = sqrt(sqr(x0) + sqr(y0));
0408     printIt(&result, "Fast_fit - result: ");
0409 
0410     // LINE FIT
0411     const Vector2d dVec = hits.block(0, 0, 2, 1) - result.head(2);
0412     const Vector2d eVec = hits.block(0, n - 1, 2, 1) - result.head(2);
0413     printIt(&eVec, "Fast_fit - e: ");
0414     printIt(&dVec, "Fast_fit - d: ");
0415     // Compute the arc-length between first and last point: L = R * theta = R * atan (tan (Theta) )
0416     auto dr = result(2) * atan2(cross2D(dVec, eVec), dVec.dot(eVec));
0417     // Simple difference in Z between last and first hit
0418     auto dz = hits(2, n - 1) - hits(2, 0);
0419 
0420     result(3) = (dr / dz);
0421 
0422 #ifdef RFIT_DEBUG
0423     printf("Fast_fit: [%f, %f, %f, %f]\n", result(0), result(1), result(2), result(3));
0424 #endif
0425   }
0426 
0427   /*!
0428     \brief Fit a generic number of 2D points with a circle using Riemann-Chernov
0429     algorithm. Covariance matrix of fitted parameter is optionally computed.
0430     Multiple scattering (currently only in barrel layer) is optionally handled.
0431     \param hits2D 2D points to be fitted.
0432     \param hits_cov2D covariance matrix of 2D points.
0433     \param fast_fit pre-fit result in this form: (X0,Y0,R,tan(theta)).
0434     (tan(theta) is not used).
0435     \param bField magnetic field
0436     \param error flag for error computation.
0437     \param scattering flag for multiple scattering
0438     \return circle circle_fit:
0439     -par parameter of the fitted circle in this form (X0,Y0,R); \n
0440     -cov covariance matrix of the fitted parameter (not initialized if
0441     error = false); \n
0442     -q charge of the particle; \n
0443     -chi2.
0444     \warning hits must be passed ordered from inner to outer layer (double hits
0445     on the same layer must be ordered too) so that multiple scattering is
0446     treated properly.
0447     \warning Multiple scattering for barrel is still not tested.
0448     \warning Multiple scattering for endcap hits is not handled (yet). Do not
0449     fit endcap hits with scattering = true !
0450     \bug for small pt (<0.3 Gev/c) chi2 could be slightly underestimated.
0451     \bug further investigation needed for error propagation with multiple
0452     scattering.
0453 */
0454   template <typename M2xN, typename V4, int N>
0455   __host__ __device__ inline CircleFit circleFit(const M2xN& hits2D,
0456                                                  const Matrix2Nd<N>& hits_cov2D,
0457                                                  const V4& fast_fit,
0458                                                  const VectorNd<N>& rad,
0459                                                  const double bField,
0460                                                  const bool error) {
0461 #ifdef RFIT_DEBUG
0462     printf("circle_fit - enter\n");
0463 #endif
0464     // INITIALIZATION
0465     Matrix2Nd<N> vMat = hits_cov2D;
0466     constexpr uint n = N;
0467     printIt(&hits2D, "circle_fit - hits2D:");
0468     printIt(&hits_cov2D, "circle_fit - hits_cov2D:");
0469 
0470 #ifdef RFIT_DEBUG
0471     printf("circle_fit - WEIGHT COMPUTATION\n");
0472 #endif
0473     // WEIGHT COMPUTATION
0474     VectorNd<N> weight;
0475     MatrixNd<N> gMat;
0476     double renorm;
0477     {
0478       MatrixNd<N> cov_rad = cov_carttorad_prefit(hits2D, vMat, fast_fit, rad).asDiagonal();
0479       MatrixNd<N> scatterCovRadMat = scatter_cov_rad(hits2D, fast_fit, rad, bField);
0480       printIt(&scatterCovRadMat, "circle_fit - scatter_cov_rad:");
0481       printIt(&hits2D, "circle_fit - hits2D bis:");
0482 #ifdef RFIT_DEBUG
0483       printf("Address of hits2D: a) %p\n", &hits2D);
0484 #endif
0485       vMat += cov_radtocart(hits2D, scatterCovRadMat, rad);
0486       printIt(&vMat, "circle_fit - V:");
0487       cov_rad += scatterCovRadMat;
0488       printIt(&cov_rad, "circle_fit - cov_rad:");
0489       math::cholesky::invert(cov_rad, gMat);
0490       // gMat = cov_rad.inverse();
0491       renorm = gMat.sum();
0492       gMat *= 1. / renorm;
0493       weight = weightCircle(gMat);
0494     }
0495     printIt(&weight, "circle_fit - weight:");
0496 
0497     // SPACE TRANSFORMATION
0498 #ifdef RFIT_DEBUG
0499     printf("circle_fit - SPACE TRANSFORMATION\n");
0500 #endif
0501 
0502     // center
0503 #ifdef RFIT_DEBUG
0504     printf("Address of hits2D: b) %p\n", &hits2D);
0505 #endif
0506     const Vector2d hCentroid = hits2D.rowwise().mean();  // centroid
0507     printIt(&hCentroid, "circle_fit - h_:");
0508     Matrix3xNd<N> p3D;
0509     p3D.block(0, 0, 2, n) = hits2D.colwise() - hCentroid;
0510     printIt(&p3D, "circle_fit - p3D: a)");
0511     Vector2Nd<N> mc;  // centered hits, used in error computation
0512     mc << p3D.row(0).transpose(), p3D.row(1).transpose();
0513     printIt(&mc, "circle_fit - mc(centered hits):");
0514 
0515     // scale
0516     const double tempQ = mc.squaredNorm();
0517     const double tempS = sqrt(n * 1. / tempQ);  // scaling factor
0518     p3D.block(0, 0, 2, n) *= tempS;
0519 
0520     // project on paraboloid
0521     p3D.row(2) = p3D.block(0, 0, 2, n).colwise().squaredNorm();
0522     printIt(&p3D, "circle_fit - p3D: b)");
0523 
0524 #ifdef RFIT_DEBUG
0525     printf("circle_fit - COST FUNCTION\n");
0526 #endif
0527     // COST FUNCTION
0528 
0529     // compute
0530     Vector3d r0;
0531     r0.noalias() = p3D * weight;  // center of gravity
0532     const Matrix3xNd<N> xMat = p3D.colwise() - r0;
0533     Matrix3d aMat = xMat * gMat * xMat.transpose();
0534     printIt(&aMat, "circle_fit - A:");
0535 
0536 #ifdef RFIT_DEBUG
0537     printf("circle_fit - MINIMIZE\n");
0538 #endif
0539     // minimize
0540     double chi2;
0541     Vector3d vVec = min_eigen3D(aMat, chi2);
0542 #ifdef RFIT_DEBUG
0543     printf("circle_fit - AFTER MIN_EIGEN\n");
0544 #endif
0545     printIt(&vVec, "v BEFORE INVERSION");
0546     vVec *= (vVec(2) > 0) ? 1 : -1;  // TO FIX dovrebbe essere N(3)>0
0547     printIt(&vVec, "v AFTER INVERSION");
0548     // This hack to be able to run on GPU where the automatic assignment to a
0549     // double from the vector multiplication is not working.
0550 #ifdef RFIT_DEBUG
0551     printf("circle_fit - AFTER MIN_EIGEN 1\n");
0552 #endif
0553     Eigen::Matrix<double, 1, 1> cm;
0554 #ifdef RFIT_DEBUG
0555     printf("circle_fit - AFTER MIN_EIGEN 2\n");
0556 #endif
0557     cm = -vVec.transpose() * r0;
0558 #ifdef RFIT_DEBUG
0559     printf("circle_fit - AFTER MIN_EIGEN 3\n");
0560 #endif
0561     const double tempC = cm(0, 0);
0562 
0563 #ifdef RFIT_DEBUG
0564     printf("circle_fit - COMPUTE CIRCLE PARAMETER\n");
0565 #endif
0566     // COMPUTE CIRCLE PARAMETER
0567 
0568     // auxiliary quantities
0569     const double tempH = sqrt(1. - sqr(vVec(2)) - 4. * tempC * vVec(2));
0570     const double v2x2_inv = 1. / (2. * vVec(2));
0571     const double s_inv = 1. / tempS;
0572     Vector3d par_uvr;  // used in error propagation
0573     par_uvr << -vVec(0) * v2x2_inv, -vVec(1) * v2x2_inv, tempH * v2x2_inv;
0574 
0575     CircleFit circle;
0576     circle.par << par_uvr(0) * s_inv + hCentroid(0), par_uvr(1) * s_inv + hCentroid(1), par_uvr(2) * s_inv;
0577     circle.qCharge = charge(hits2D, circle.par);
0578     circle.chi2 = abs(chi2) * renorm / sqr(2 * vVec(2) * par_uvr(2) * tempS);
0579     printIt(&circle.par, "circle_fit - CIRCLE PARAMETERS:");
0580     printIt(&circle.cov, "circle_fit - CIRCLE COVARIANCE:");
0581 #ifdef RFIT_DEBUG
0582     printf("circle_fit - CIRCLE CHARGE: %d\n", circle.qCharge);
0583 #endif
0584 
0585 #ifdef RFIT_DEBUG
0586     printf("circle_fit - ERROR PROPAGATION\n");
0587 #endif
0588     // ERROR PROPAGATION
0589     if (error) {
0590 #ifdef RFIT_DEBUG
0591       printf("circle_fit - ERROR PRPAGATION ACTIVATED\n");
0592 #endif
0593       ArrayNd<N> vcsMat[2][2];  // cov matrix of center & scaled points
0594       MatrixNd<N> cMat[3][3];   // cov matrix of 3D transformed points
0595 #ifdef RFIT_DEBUG
0596       printf("circle_fit - ERROR PRPAGATION ACTIVATED 2\n");
0597 #endif
0598       {
0599         Eigen::Matrix<double, 1, 1> cm;
0600         Eigen::Matrix<double, 1, 1> cm2;
0601         cm = mc.transpose() * vMat * mc;
0602         const double tempC2 = cm(0, 0);
0603         Matrix2Nd<N> tempVcsMat;
0604         tempVcsMat.template triangularView<Eigen::Upper>() =
0605             (sqr(tempS) * vMat + sqr(sqr(tempS)) * 1. / (4. * tempQ * n) *
0606                                      (2. * vMat.squaredNorm() + 4. * tempC2) *  // mc.transpose() * V * mc) *
0607                                      (mc * mc.transpose()));
0608 
0609         printIt(&tempVcsMat, "circle_fit - Vcs:");
0610         cMat[0][0] = tempVcsMat.block(0, 0, n, n).template selfadjointView<Eigen::Upper>();
0611         vcsMat[0][1] = tempVcsMat.block(0, n, n, n);
0612         cMat[1][1] = tempVcsMat.block(n, n, n, n).template selfadjointView<Eigen::Upper>();
0613         vcsMat[1][0] = vcsMat[0][1].transpose();
0614         printIt(&tempVcsMat, "circle_fit - Vcs:");
0615       }
0616 
0617       {
0618         const ArrayNd<N> t0 = (VectorXd::Constant(n, 1.) * p3D.row(0));
0619         const ArrayNd<N> t1 = (VectorXd::Constant(n, 1.) * p3D.row(1));
0620         const ArrayNd<N> t00 = p3D.row(0).transpose() * p3D.row(0);
0621         const ArrayNd<N> t01 = p3D.row(0).transpose() * p3D.row(1);
0622         const ArrayNd<N> t11 = p3D.row(1).transpose() * p3D.row(1);
0623         const ArrayNd<N> t10 = t01.transpose();
0624         vcsMat[0][0] = cMat[0][0];
0625         cMat[0][1] = vcsMat[0][1];
0626         cMat[0][2] = 2. * (vcsMat[0][0] * t0 + vcsMat[0][1] * t1);
0627         vcsMat[1][1] = cMat[1][1];
0628         cMat[1][2] = 2. * (vcsMat[1][0] * t0 + vcsMat[1][1] * t1);
0629         MatrixNd<N> tmp;
0630         tmp.template triangularView<Eigen::Upper>() =
0631             (2. * (vcsMat[0][0] * vcsMat[0][0] + vcsMat[0][0] * vcsMat[0][1] + vcsMat[1][1] * vcsMat[1][0] +
0632                    vcsMat[1][1] * vcsMat[1][1]) +
0633              4. * (vcsMat[0][0] * t00 + vcsMat[0][1] * t01 + vcsMat[1][0] * t10 + vcsMat[1][1] * t11))
0634                 .matrix();
0635         cMat[2][2] = tmp.template selfadjointView<Eigen::Upper>();
0636       }
0637       printIt(&cMat[0][0], "circle_fit - C[0][0]:");
0638 
0639       Matrix3d c0Mat;  // cov matrix of center of gravity (r0.x,r0.y,r0.z)
0640       for (uint i = 0; i < 3; ++i) {
0641         for (uint j = i; j < 3; ++j) {
0642           Eigen::Matrix<double, 1, 1> tmp;
0643           tmp = weight.transpose() * cMat[i][j] * weight;
0644           // Workaround to get things working in GPU
0645           const double tempC = tmp(0, 0);
0646           c0Mat(i, j) = tempC;  //weight.transpose() * C[i][j] * weight;
0647           c0Mat(j, i) = c0Mat(i, j);
0648         }
0649       }
0650       printIt(&c0Mat, "circle_fit - C0:");
0651 
0652       const MatrixNd<N> wMat = weight * weight.transpose();
0653       const MatrixNd<N> hMat = MatrixNd<N>::Identity().rowwise() - weight.transpose();
0654       const MatrixNx3d<N> s_v = hMat * p3D.transpose();
0655       printIt(&wMat, "circle_fit - W:");
0656       printIt(&hMat, "circle_fit - H:");
0657       printIt(&s_v, "circle_fit - s_v:");
0658 
0659       MatrixNd<N> dMat[3][3];  // cov(s_v)
0660       dMat[0][0] = (hMat * cMat[0][0] * hMat.transpose()).cwiseProduct(wMat);
0661       dMat[0][1] = (hMat * cMat[0][1] * hMat.transpose()).cwiseProduct(wMat);
0662       dMat[0][2] = (hMat * cMat[0][2] * hMat.transpose()).cwiseProduct(wMat);
0663       dMat[1][1] = (hMat * cMat[1][1] * hMat.transpose()).cwiseProduct(wMat);
0664       dMat[1][2] = (hMat * cMat[1][2] * hMat.transpose()).cwiseProduct(wMat);
0665       dMat[2][2] = (hMat * cMat[2][2] * hMat.transpose()).cwiseProduct(wMat);
0666       dMat[1][0] = dMat[0][1].transpose();
0667       dMat[2][0] = dMat[0][2].transpose();
0668       dMat[2][1] = dMat[1][2].transpose();
0669       printIt(&dMat[0][0], "circle_fit - D_[0][0]:");
0670 
0671       constexpr uint nu[6][2] = {{0, 0}, {0, 1}, {0, 2}, {1, 1}, {1, 2}, {2, 2}};
0672 
0673       Matrix6d eMat;  // cov matrix of the 6 independent elements of A
0674       for (uint a = 0; a < 6; ++a) {
0675         const uint i = nu[a][0], j = nu[a][1];
0676         for (uint b = a; b < 6; ++b) {
0677           const uint k = nu[b][0], l = nu[b][1];
0678           VectorNd<N> t0(n);
0679           VectorNd<N> t1(n);
0680           if (l == k) {
0681             t0 = 2. * dMat[j][l] * s_v.col(l);
0682             if (i == j)
0683               t1 = t0;
0684             else
0685               t1 = 2. * dMat[i][l] * s_v.col(l);
0686           } else {
0687             t0 = dMat[j][l] * s_v.col(k) + dMat[j][k] * s_v.col(l);
0688             if (i == j)
0689               t1 = t0;
0690             else
0691               t1 = dMat[i][l] * s_v.col(k) + dMat[i][k] * s_v.col(l);
0692           }
0693 
0694           if (i == j) {
0695             Eigen::Matrix<double, 1, 1> cm;
0696             cm = s_v.col(i).transpose() * (t0 + t1);
0697             // Workaround to get things working in GPU
0698             const double tempC = cm(0, 0);
0699             eMat(a, b) = 0. + tempC;
0700           } else {
0701             Eigen::Matrix<double, 1, 1> cm;
0702             cm = (s_v.col(i).transpose() * t0) + (s_v.col(j).transpose() * t1);
0703             // Workaround to get things working in GPU
0704             const double tempC = cm(0, 0);
0705             eMat(a, b) = 0. + tempC;  //(s_v.col(i).transpose() * t0) + (s_v.col(j).transpose() * t1);
0706           }
0707           if (b != a)
0708             eMat(b, a) = eMat(a, b);
0709         }
0710       }
0711       printIt(&eMat, "circle_fit - E:");
0712 
0713       Eigen::Matrix<double, 3, 6> j2Mat;  // Jacobian of min_eigen() (numerically computed)
0714       for (uint a = 0; a < 6; ++a) {
0715         const uint i = nu[a][0], j = nu[a][1];
0716         Matrix3d delta = Matrix3d::Zero();
0717         delta(i, j) = delta(j, i) = abs(aMat(i, j) * epsilon);
0718         j2Mat.col(a) = min_eigen3D_fast(aMat + delta);
0719         const int sign = (j2Mat.col(a)(2) > 0) ? 1 : -1;
0720         j2Mat.col(a) = (j2Mat.col(a) * sign - vVec) / delta(i, j);
0721       }
0722       printIt(&j2Mat, "circle_fit - J2:");
0723 
0724       Matrix4d cvcMat;  // joint cov matrix of (v0,v1,v2,c)
0725       {
0726         Matrix3d t0 = j2Mat * eMat * j2Mat.transpose();
0727         Vector3d t1 = -t0 * r0;
0728         cvcMat.block(0, 0, 3, 3) = t0;
0729         cvcMat.block(0, 3, 3, 1) = t1;
0730         cvcMat.block(3, 0, 1, 3) = t1.transpose();
0731         Eigen::Matrix<double, 1, 1> cm1;
0732         Eigen::Matrix<double, 1, 1> cm3;
0733         cm1 = (vVec.transpose() * c0Mat * vVec);
0734         //      cm2 = (c0Mat.cwiseProduct(t0)).sum();
0735         cm3 = (r0.transpose() * t0 * r0);
0736         // Workaround to get things working in GPU
0737         const double tempC = cm1(0, 0) + (c0Mat.cwiseProduct(t0)).sum() + cm3(0, 0);
0738         cvcMat(3, 3) = tempC;
0739         // (v.transpose() * c0Mat * v) + (c0Mat.cwiseProduct(t0)).sum() + (r0.transpose() * t0 * r0);
0740       }
0741       printIt(&cvcMat, "circle_fit - Cvc:");
0742 
0743       Eigen::Matrix<double, 3, 4> j3Mat;  // Jacobian (v0,v1,v2,c)->(X0,Y0,R)
0744       {
0745         const double t = 1. / tempH;
0746         j3Mat << -v2x2_inv, 0, vVec(0) * sqr(v2x2_inv) * 2., 0, 0, -v2x2_inv, vVec(1) * sqr(v2x2_inv) * 2., 0,
0747             vVec(0) * v2x2_inv * t, vVec(1) * v2x2_inv * t,
0748             -tempH * sqr(v2x2_inv) * 2. - (2. * tempC + vVec(2)) * v2x2_inv * t, -t;
0749       }
0750       printIt(&j3Mat, "circle_fit - J3:");
0751 
0752       const RowVector2Nd<N> Jq = mc.transpose() * tempS * 1. / n;  // var(q)
0753       printIt(&Jq, "circle_fit - Jq:");
0754 
0755       Matrix3d cov_uvr = j3Mat * cvcMat * j3Mat.transpose() * sqr(s_inv)  // cov(X0,Y0,R)
0756                          + (par_uvr * par_uvr.transpose()) * (Jq * vMat * Jq.transpose());
0757 
0758       circle.cov = cov_uvr;
0759     }
0760 
0761     printIt(&circle.cov, "Circle cov:");
0762 #ifdef RFIT_DEBUG
0763     printf("circle_fit - exit\n");
0764 #endif
0765     return circle;
0766   }
0767 
0768   /*!  \brief Perform an ordinary least square fit in the s-z plane to compute
0769  * the parameters cotTheta and Zip.
0770  *
0771  * The fit is performed in the rotated S3D-Z' plane, following the formalism of
0772  * Frodesen, Chapter 10, p. 259.
0773  *
0774  * The system has been rotated to both try to use the combined errors in s-z
0775  * along Z', as errors in the Y direction and to avoid the patological case of
0776  * degenerate lines with angular coefficient m = +/- inf.
0777  *
0778  * The rotation is using the information on the theta angle computed in the
0779  * fast fit. The rotation is such that the S3D axis will be the X-direction,
0780  * while the rotated Z-axis will be the Y-direction. This pretty much follows
0781  * what is done in the same fit in the Broken Line approach.
0782  */
0783 
0784   template <typename M3xN, typename M6xN, typename V4>
0785   __host__ __device__ inline LineFit lineFit(const M3xN& hits,
0786                                              const M6xN& hits_ge,
0787                                              const CircleFit& circle,
0788                                              const V4& fast_fit,
0789                                              const double bField,
0790                                              const bool error) {
0791     constexpr uint32_t N = M3xN::ColsAtCompileTime;
0792     constexpr auto n = N;
0793     double theta = -circle.qCharge * atan(fast_fit(3));
0794     theta = theta < 0. ? theta + M_PI : theta;
0795 
0796     // Prepare the Rotation Matrix to rotate the points
0797     Eigen::Matrix<double, 2, 2> rot;
0798     rot << sin(theta), cos(theta), -cos(theta), sin(theta);
0799 
0800     // PROJECTION ON THE CILINDER
0801     //
0802     // p2D will be:
0803     // [s1, s2, s3, ..., sn]
0804     // [z1, z2, z3, ..., zn]
0805     // s values will be ordinary x-values
0806     // z values will be ordinary y-values
0807 
0808     Matrix2xNd<N> p2D = Matrix2xNd<N>::Zero();
0809     Eigen::Matrix<double, 2, 6> jxMat;
0810 
0811 #ifdef RFIT_DEBUG
0812     printf("Line_fit - B: %g\n", bField);
0813     printIt(&hits, "Line_fit points: ");
0814     printIt(&hits_ge, "Line_fit covs: ");
0815     printIt(&rot, "Line_fit rot: ");
0816 #endif
0817     // x & associated Jacobian
0818     // cfr https://indico.cern.ch/event/663159/contributions/2707659/attachments/1517175/2368189/Riemann_fit.pdf
0819     // Slide 11
0820     // a ==> -o i.e. the origin of the circle in XY plane, negative
0821     // b ==> p i.e. distances of the points wrt the origin of the circle.
0822     const Vector2d oVec(circle.par(0), circle.par(1));
0823 
0824     // associated Jacobian, used in weights and errors computation
0825     Matrix6d covMat = Matrix6d::Zero();
0826     Matrix2d cov_sz[N];
0827     for (uint i = 0; i < n; ++i) {
0828       Vector2d pVec = hits.block(0, i, 2, 1) - oVec;
0829       const double cross = cross2D(-oVec, pVec);
0830       const double dot = (-oVec).dot(pVec);
0831       // atan2(cross, dot) give back the angle in the transverse plane so tha the
0832       // final equation reads: x_i = -q*R*theta (theta = angle returned by atan2)
0833       const double tempQAtan2 = -circle.qCharge * atan2(cross, dot);
0834       //    p2D.coeffRef(1, i) = atan2_ * circle.par(2);
0835       p2D(0, i) = tempQAtan2 * circle.par(2);
0836 
0837       // associated Jacobian, used in weights and errors- computation
0838       const double temp0 = -circle.qCharge * circle.par(2) * 1. / (sqr(dot) + sqr(cross));
0839       double d_X0 = 0., d_Y0 = 0., d_R = 0.;  // good approximation for big pt and eta
0840       if (error) {
0841         d_X0 = -temp0 * ((pVec(1) + oVec(1)) * dot - (pVec(0) - oVec(0)) * cross);
0842         d_Y0 = temp0 * ((pVec(0) + oVec(0)) * dot - (oVec(1) - pVec(1)) * cross);
0843         d_R = tempQAtan2;
0844       }
0845       const double d_x = temp0 * (oVec(1) * dot + oVec(0) * cross);
0846       const double d_y = temp0 * (-oVec(0) * dot + oVec(1) * cross);
0847       jxMat << d_X0, d_Y0, d_R, d_x, d_y, 0., 0., 0., 0., 0., 0., 1.;
0848 
0849       covMat.block(0, 0, 3, 3) = circle.cov;
0850       covMat(3, 3) = hits_ge.col(i)[0];                 // x errors
0851       covMat(4, 4) = hits_ge.col(i)[2];                 // y errors
0852       covMat(5, 5) = hits_ge.col(i)[5];                 // z errors
0853       covMat(3, 4) = covMat(4, 3) = hits_ge.col(i)[1];  // cov_xy
0854       covMat(3, 5) = covMat(5, 3) = hits_ge.col(i)[3];  // cov_xz
0855       covMat(4, 5) = covMat(5, 4) = hits_ge.col(i)[4];  // cov_yz
0856       Matrix2d tmp = jxMat * covMat * jxMat.transpose();
0857       cov_sz[i].noalias() = rot * tmp * rot.transpose();
0858     }
0859     // Math of d_{X0,Y0,R,x,y} all verified by hand
0860     p2D.row(1) = hits.row(2);
0861 
0862     // The following matrix will contain errors orthogonal to the rotated S
0863     // component only, with the Multiple Scattering properly treated!!
0864     MatrixNd<N> cov_with_ms;
0865     scatterCovLine(cov_sz, fast_fit, p2D.row(0), p2D.row(1), theta, bField, cov_with_ms);
0866 #ifdef RFIT_DEBUG
0867     printIt(cov_sz, "line_fit - cov_sz:");
0868     printIt(&cov_with_ms, "line_fit - cov_with_ms: ");
0869 #endif
0870 
0871     // Rotate Points with the shape [2, n]
0872     Matrix2xNd<N> p2D_rot = rot * p2D;
0873 
0874 #ifdef RFIT_DEBUG
0875     printf("Fast fit Tan(theta): %g\n", fast_fit(3));
0876     printf("Rotation angle: %g\n", theta);
0877     printIt(&rot, "Rotation Matrix:");
0878     printIt(&p2D, "Original Hits(s,z):");
0879     printIt(&p2D_rot, "Rotated hits(S3D, Z'):");
0880     printIt(&rot, "Rotation Matrix:");
0881 #endif
0882 
0883     // Build the A Matrix
0884     Matrix2xNd<N> aMat;
0885     aMat << MatrixXd::Ones(1, n), p2D_rot.row(0);  // rotated s values
0886 
0887 #ifdef RFIT_DEBUG
0888     printIt(&aMat, "A Matrix:");
0889 #endif
0890 
0891     // Build A^T V-1 A, where V-1 is the covariance of only the Y components.
0892     MatrixNd<N> vyInvMat;
0893     math::cholesky::invert(cov_with_ms, vyInvMat);
0894     // MatrixNd<N> vyInvMat = cov_with_ms.inverse();
0895     Eigen::Matrix<double, 2, 2> covParamsMat = aMat * vyInvMat * aMat.transpose();
0896     // Compute the Covariance Matrix of the fit parameters
0897     math::cholesky::invert(covParamsMat, covParamsMat);
0898 
0899     // Now Compute the Parameters in the form [2,1]
0900     // The first component is q.
0901     // The second component is m.
0902     Eigen::Matrix<double, 2, 1> sol = covParamsMat * aMat * vyInvMat * p2D_rot.row(1).transpose();
0903 
0904 #ifdef RFIT_DEBUG
0905     printIt(&sol, "Rotated solutions:");
0906 #endif
0907 
0908     // We need now to transfer back the results in the original s-z plane
0909     const auto sinTheta = sin(theta);
0910     const auto cosTheta = cos(theta);
0911     auto common_factor = 1. / (sinTheta - sol(1, 0) * cosTheta);
0912     Eigen::Matrix<double, 2, 2> jMat;
0913     jMat << 0., common_factor * common_factor, common_factor, sol(0, 0) * cosTheta * common_factor * common_factor;
0914 
0915     double tempM = common_factor * (sol(1, 0) * sinTheta + cosTheta);
0916     double tempQ = common_factor * sol(0, 0);
0917     auto cov_mq = jMat * covParamsMat * jMat.transpose();
0918 
0919     VectorNd<N> res = p2D_rot.row(1).transpose() - aMat.transpose() * sol;
0920     double chi2 = res.transpose() * vyInvMat * res;
0921 
0922     LineFit line;
0923     line.par << tempM, tempQ;
0924     line.cov << cov_mq;
0925     line.chi2 = chi2;
0926 
0927 #ifdef RFIT_DEBUG
0928     printf("Common_factor: %g\n", common_factor);
0929     printIt(&jMat, "Jacobian:");
0930     printIt(&sol, "Rotated solutions:");
0931     printIt(&covParamsMat, "Cov_params:");
0932     printIt(&cov_mq, "Rotated Covariance Matrix:");
0933     printIt(&(line.par), "Real Parameters:");
0934     printIt(&(line.cov), "Real Covariance Matrix:");
0935     printf("Chi2: %g\n", chi2);
0936 #endif
0937 
0938     return line;
0939   }
0940 
0941   /*!
0942     \brief Helix fit by three step:
0943     -fast pre-fit (see Fast_fit() for further info); \n
0944     -circle fit of hits projected in the transverse plane by Riemann-Chernov
0945         algorithm (see Circle_fit() for further info); \n
0946     -line fit of hits projected on cylinder surface by orthogonal distance
0947         regression (see Line_fit for further info). \n
0948     Points must be passed ordered (from inner to outer layer).
0949     \param hits Matrix3xNd hits coordinates in this form: \n
0950         |x0|x1|x2|...|xn| \n
0951         |y0|y1|y2|...|yn| \n
0952         |z0|z1|z2|...|zn|
0953     \param hits_cov Matrix3Nd covariance matrix in this form (()->cov()): \n
0954    |(x0,x0)|(x1,x0)|(x2,x0)|.|(y0,x0)|(y1,x0)|(y2,x0)|.|(z0,x0)|(z1,x0)|(z2,x0)| \n
0955    |(x0,x1)|(x1,x1)|(x2,x1)|.|(y0,x1)|(y1,x1)|(y2,x1)|.|(z0,x1)|(z1,x1)|(z2,x1)| \n
0956    |(x0,x2)|(x1,x2)|(x2,x2)|.|(y0,x2)|(y1,x2)|(y2,x2)|.|(z0,x2)|(z1,x2)|(z2,x2)| \n
0957        .       .       .    .    .       .       .    .    .       .       .     \n
0958    |(x0,y0)|(x1,y0)|(x2,y0)|.|(y0,y0)|(y1,y0)|(y2,x0)|.|(z0,y0)|(z1,y0)|(z2,y0)| \n
0959    |(x0,y1)|(x1,y1)|(x2,y1)|.|(y0,y1)|(y1,y1)|(y2,x1)|.|(z0,y1)|(z1,y1)|(z2,y1)| \n
0960    |(x0,y2)|(x1,y2)|(x2,y2)|.|(y0,y2)|(y1,y2)|(y2,x2)|.|(z0,y2)|(z1,y2)|(z2,y2)| \n
0961        .       .       .    .    .       .       .    .    .       .       .     \n
0962    |(x0,z0)|(x1,z0)|(x2,z0)|.|(y0,z0)|(y1,z0)|(y2,z0)|.|(z0,z0)|(z1,z0)|(z2,z0)| \n
0963    |(x0,z1)|(x1,z1)|(x2,z1)|.|(y0,z1)|(y1,z1)|(y2,z1)|.|(z0,z1)|(z1,z1)|(z2,z1)| \n
0964    |(x0,z2)|(x1,z2)|(x2,z2)|.|(y0,z2)|(y1,z2)|(y2,z2)|.|(z0,z2)|(z1,z2)|(z2,z2)|
0965    \param bField magnetic field in the center of the detector in Gev/cm/c
0966    unit, in order to perform pt calculation.
0967    \param error flag for error computation.
0968    \param scattering flag for multiple scattering treatment.
0969    (see Circle_fit() documentation for further info).
0970    \warning see Circle_fit(), Line_fit() and Fast_fit() warnings.
0971    \bug see Circle_fit(), Line_fit() and Fast_fit() bugs.
0972 */
0973 
0974   template <int N>
0975   inline HelixFit helixFit(const Matrix3xNd<N>& hits,
0976                            const Eigen::Matrix<float, 6, N>& hits_ge,
0977                            const double bField,
0978                            const bool error) {
0979     constexpr uint n = N;
0980     VectorNd<4> rad = (hits.block(0, 0, 2, n).colwise().norm());
0981 
0982     // Fast_fit gives back (X0, Y0, R, theta) w/o errors, using only 3 points.
0983     Vector4d fast_fit;
0984     fastFit(hits, fast_fit);
0985     riemannFit::Matrix2Nd<N> hits_cov = MatrixXd::Zero(2 * n, 2 * n);
0986     riemannFit::loadCovariance2D(hits_ge, hits_cov);
0987     CircleFit circle = circleFit(hits.block(0, 0, 2, n), hits_cov, fast_fit, rad, bField, error);
0988     LineFit line = lineFit(hits, hits_ge, circle, fast_fit, bField, error);
0989 
0990     par_uvrtopak(circle, bField, error);
0991 
0992     HelixFit helix;
0993     helix.par << circle.par, line.par;
0994     if (error) {
0995       helix.cov = MatrixXd::Zero(5, 5);
0996       helix.cov.block(0, 0, 3, 3) = circle.cov;
0997       helix.cov.block(3, 3, 2, 2) = line.cov;
0998     }
0999     helix.qCharge = circle.qCharge;
1000     helix.chi2_circle = circle.chi2;
1001     helix.chi2_line = line.chi2;
1002 
1003     return helix;
1004   }
1005 
1006 }  // namespace riemannFit
1007 
1008 #endif  // RecoPixelVertexing_PixelTrackFitting_interface_RiemannFit_h