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
0009
0010
0011
0012
0013
0014
0015
0016
0017
0018
0019
0020
0021
0022
0023
0024
0025
0026
0027
0028
0029
0030 template <typename VNd1, typename VNd2>
0031 __host__ __device__ inline void computeRadLenUniformMaterial(const VNd1& length_values, VNd2& rad_lengths) {
0032
0033
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
0044
0045
0046
0047
0048
0049
0050
0051
0052
0053
0054
0055
0056
0057
0058
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);
0074 double p_2 = p_t * p_t * (1. + 1. / sqr(fast_fit(3)));
0075 VectorNd<N> rad_lengths_S;
0076
0077
0078
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
0102
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
0111
0112
0113
0114
0115
0116
0117
0118
0119
0120
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);
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
0137 for (uint i = 0; i < n; ++i) {
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
0164
0165
0166
0167
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
0199
0200
0201
0202
0203
0204
0205
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
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
0228
0229
0230
0231
0232
0233
0234
0235
0236
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
0247 if (rad(i) < 1.e-4)
0248 cov_rad(i) = cov_cart(i, i);
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
0265
0266
0267
0268
0269
0270
0271
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
0281
0282
0283
0284
0285
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
0297
0298
0299
0300
0301
0302
0303
0304
0305
0306
0307
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
0326
0327
0328
0329
0330
0331
0332
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
0345
0346
0347
0348
0349
0350
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
0363
0364
0365
0366
0367
0368
0369
0370
0371
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;
0378 printIt(&hits, "Fast_fit - hits: ");
0379
0380
0381
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
0387 auto b2 = bVec.squaredNorm();
0388 auto c2 = cVec.squaredNorm();
0389
0390
0391
0392
0393
0394
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
0401 auto div = 2. * (cx * by - bx * cy);
0402
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
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
0416 auto dr = result(2) * atan2(cross2D(dVec, eVec), dVec.dot(eVec));
0417
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
0429
0430
0431
0432
0433
0434
0435
0436
0437
0438
0439
0440
0441
0442
0443
0444
0445
0446
0447
0448
0449
0450
0451
0452
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
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
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
0491 renorm = gMat.sum();
0492 gMat *= 1. / renorm;
0493 weight = weightCircle(gMat);
0494 }
0495 printIt(&weight, "circle_fit - weight:");
0496
0497
0498 #ifdef RFIT_DEBUG
0499 printf("circle_fit - SPACE TRANSFORMATION\n");
0500 #endif
0501
0502
0503 #ifdef RFIT_DEBUG
0504 printf("Address of hits2D: b) %p\n", &hits2D);
0505 #endif
0506 const Vector2d hCentroid = hits2D.rowwise().mean();
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;
0512 mc << p3D.row(0).transpose(), p3D.row(1).transpose();
0513 printIt(&mc, "circle_fit - mc(centered hits):");
0514
0515
0516 const double tempQ = mc.squaredNorm();
0517 const double tempS = sqrt(n * 1. / tempQ);
0518 p3D.block(0, 0, 2, n) *= tempS;
0519
0520
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
0528
0529
0530 Vector3d r0;
0531 r0.noalias() = p3D * weight;
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
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;
0547 printIt(&vVec, "v AFTER INVERSION");
0548
0549
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
0567
0568
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;
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
0589 if (error) {
0590 #ifdef RFIT_DEBUG
0591 printf("circle_fit - ERROR PRPAGATION ACTIVATED\n");
0592 #endif
0593 ArrayNd<N> vcsMat[2][2];
0594 MatrixNd<N> cMat[3][3];
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) *
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;
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
0645 const double tempC = tmp(0, 0);
0646 c0Mat(i, j) = tempC;
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];
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;
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
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
0704 const double tempC = cm(0, 0);
0705 eMat(a, b) = 0. + tempC;
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;
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;
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
0735 cm3 = (r0.transpose() * t0 * r0);
0736
0737 const double tempC = cm1(0, 0) + (c0Mat.cwiseProduct(t0)).sum() + cm3(0, 0);
0738 cvcMat(3, 3) = tempC;
0739
0740 }
0741 printIt(&cvcMat, "circle_fit - Cvc:");
0742
0743 Eigen::Matrix<double, 3, 4> j3Mat;
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;
0753 printIt(&Jq, "circle_fit - Jq:");
0754
0755 Matrix3d cov_uvr = j3Mat * cvcMat * j3Mat.transpose() * sqr(s_inv)
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
0769
0770
0771
0772
0773
0774
0775
0776
0777
0778
0779
0780
0781
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
0797 Eigen::Matrix<double, 2, 2> rot;
0798 rot << sin(theta), cos(theta), -cos(theta), sin(theta);
0799
0800
0801
0802
0803
0804
0805
0806
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
0818
0819
0820
0821
0822 const Vector2d oVec(circle.par(0), circle.par(1));
0823
0824
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
0832
0833 const double tempQAtan2 = -circle.qCharge * atan2(cross, dot);
0834
0835 p2D(0, i) = tempQAtan2 * circle.par(2);
0836
0837
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.;
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];
0851 covMat(4, 4) = hits_ge.col(i)[2];
0852 covMat(5, 5) = hits_ge.col(i)[5];
0853 covMat(3, 4) = covMat(4, 3) = hits_ge.col(i)[1];
0854 covMat(3, 5) = covMat(5, 3) = hits_ge.col(i)[3];
0855 covMat(4, 5) = covMat(5, 4) = hits_ge.col(i)[4];
0856 Matrix2d tmp = jxMat * covMat * jxMat.transpose();
0857 cov_sz[i].noalias() = rot * tmp * rot.transpose();
0858 }
0859
0860 p2D.row(1) = hits.row(2);
0861
0862
0863
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
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
0884 Matrix2xNd<N> aMat;
0885 aMat << MatrixXd::Ones(1, n), p2D_rot.row(0);
0886
0887 #ifdef RFIT_DEBUG
0888 printIt(&aMat, "A Matrix:");
0889 #endif
0890
0891
0892 MatrixNd<N> vyInvMat;
0893 math::cholesky::invert(cov_with_ms, vyInvMat);
0894
0895 Eigen::Matrix<double, 2, 2> covParamsMat = aMat * vyInvMat * aMat.transpose();
0896
0897 math::cholesky::invert(covParamsMat, covParamsMat);
0898
0899
0900
0901
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
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
0943
0944
0945
0946
0947
0948
0949
0950
0951
0952
0953
0954
0955
0956
0957
0958
0959
0960
0961
0962
0963
0964
0965
0966
0967
0968
0969
0970
0971
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
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 }
1007
1008 #endif