1 #ifndef RecoPixelVertexing_PixelTrackFitting_interface_BrokenLine_h 2 #define RecoPixelVertexing_PixelTrackFitting_interface_BrokenLine_h 4 #include <Eigen/Eigenvalues> 58 constexpr
double inv_X0 = 0.06 / 16.;
64 constexpr
double geometry_factor = 0.7;
102 using scalar = std::remove_reference<decltype(circle.
par(0))>::
type;
114 scalar tempSmallU = 1 + rho * dee;
122 scalar tempV = 1. + rho * deltaOrth;
124 scalar mu = 1. / (tempU * (1. + tempU)) + rho * lambda;
127 2. *
mu * tempSmallU * deltaPara, 2. *
mu * tempV,
mu * zeta - lambda * tempA, 0, 0, 1.;
131 circle.
par(0) = atan2(tempB, tempC);
133 circle.
par(1) = tempA / (1 + tempU);
138 circle.
cov = jacobian * circle.
cov * jacobian.transpose();
149 template <
typename M3xN,
typename V4,
int n>
159 if constexpr (
n > 3) {
161 auto d1 = (
hits.block(0,
n / 2, 2, 1) - middle).squaredNorm();
162 auto d2 = (
hits.block(0,
n / 2 - 1, 2, 1) - middle).squaredNorm();
163 mId =
d1 < d2 ?
n / 2 :
n / 2 - 1;
166 dVec =
hits.block(0, mId, 2, 1) -
hits.block(0, 0, 2, 1);
167 eVec =
hits.block(0,
n - 1, 2, 1) -
hits.block(0, mId, 2, 1);
176 eVec = -fast_fit(2) * fast_fit.head(2) / fast_fit.head(2).norm();
177 for (u_int
i = 0;
i <
n;
i++) {
178 dVec =
results.radii.block(0,
i, 2, 1);
186 for (u_int
i = 0;
i <
n;
i++) {
188 pointsSZ(1,
i) = zVec(
i);
189 pointsSZ.block(0,
i, 2, 1) = rotMat * pointsSZ.block(0,
i, 2, 1);
191 results.sTotal = pointsSZ.block(0, 0, 1,
n).transpose();
192 results.zInSZplane = pointsSZ.block(1, 0, 1,
n).transpose();
196 for (u_int
i = 1;
i <
n - 1;
i++) {
219 for (u_int
i = 0;
i <
n;
i++) {
223 if (
i > 0 &&
i <
n - 1)
226 ((sTotal(
i + 1) - sTotal(
i)) * (sTotal(
i) - sTotal(
i - 1))));
230 if (
i > 0 &&
i <
n - 1)
232 1. / (varBeta(
i) * (sTotal(
i + 1) - sTotal(
i))) *
233 (-(sTotal(
i + 1) - sTotal(
i - 1)) / ((sTotal(
i + 1) - sTotal(
i)) * (sTotal(
i) - sTotal(
i - 1))));
236 1. / (varBeta(
i + 1) * (sTotal(
i + 1) - sTotal(
i))) *
237 (-(sTotal(
i + 2) - sTotal(
i)) / ((sTotal(
i + 2) - sTotal(
i + 1)) * (sTotal(
i + 1) - sTotal(
i))));
240 c_uMat(
i,
i + 2) = 1. / (varBeta(
i + 1) * (sTotal(
i + 2) - sTotal(
i + 1)) * (sTotal(
i + 1) - sTotal(
i)));
244 return c_uMat + c_uMat.transpose();
257 template <
typename M3xN,
typename V4>
259 constexpr uint32_t
n = M3xN::ColsAtCompileTime;
263 if constexpr (
n > 3) {
265 auto d1 = (
hits.block(0,
n / 2, 2, 1) - middle).squaredNorm();
266 auto d2 = (
hits.block(0,
n / 2 - 1, 2, 1) - middle).squaredNorm();
267 mId =
d1 < d2 ?
n / 2 :
n / 2 - 1;
275 result(0) =
hits(0, 0) - (
a(1) *
c.squaredNorm() +
c(1) *
a.squaredNorm()) *
tmp;
276 result(1) =
hits(1, 0) + (
a(0) *
c.squaredNorm() +
c(0) *
a.squaredNorm()) *
tmp;
313 template <
typename M3xN,
typename M6xN,
typename V4,
int n>
321 auto& radii =
data.radii;
322 const auto& sTransverse =
data.sTransverse;
323 const auto& sTotal =
data.sTotal;
324 auto& zInSZplane =
data.zInSZplane;
325 auto& varBeta =
data.varBeta;
326 const double slope = -circle_results.
qCharge / fast_fit(3);
329 for (u_int
i = 0;
i <
n;
i++) {
330 zInSZplane(
i) = radii.block(0,
i, 2, 1).norm() - fast_fit(2);
336 for (u_int
i = 0;
i <
n;
i++) {
337 vMat(0, 0) = hits_ge.col(
i)[0];
338 vMat(0, 1) = vMat(1, 0) = hits_ge.col(
i)[1];
339 vMat(1, 1) = hits_ge.col(
i)[2];
342 1. / ((rotMat * vMat * rotMat.transpose())(1, 1));
347 for (u_int
i = 0;
i <
n;
i++) {
348 r_uVec(
i) = weightsVec(
i) * zInSZplane(
i);
352 c_uMat.block(0, 0,
n,
n) =
matrixC_u(weightsVec, sTransverse, varBeta);
355 for (u_int
i = 0;
i <
n;
i++) {
357 if (
i > 0 &&
i <
n - 1) {
359 -(sTransverse(
i + 1) - sTransverse(
i - 1)) * (sTransverse(
i + 1) - sTransverse(
i - 1)) /
360 (2. * varBeta(
i) * (sTransverse(
i + 1) - sTransverse(
i)) * (sTransverse(
i) - sTransverse(
i - 1)));
364 (sTransverse(
i) - sTransverse(
i - 2)) / (2. * varBeta(
i - 1) * (sTransverse(
i) - sTransverse(
i - 1)));
368 (sTransverse(
i + 2) - sTransverse(
i)) / (2. * varBeta(
i + 1) * (sTransverse(
i + 1) - sTransverse(
i)));
370 c_uMat(
n,
i) = c_uMat(
i,
n);
371 if (
i > 0 &&
i <
n - 1)
376 std::cout <<
"CU5\n" << c_uMat << std::endl;
381 std::cout <<
"I5\n" << iMat << std::endl;
388 radii.block(0, 0, 2, 1) /= radii.block(0, 0, 2, 1).norm();
389 radii.block(0, 1, 2, 1) /= radii.block(0, 1, 2, 1).norm();
393 auto eMinusd = eVec - dVec;
394 auto eMinusd2 = eMinusd.squaredNorm();
395 auto tmp1 = 1. / eMinusd2;
398 circle_results.
par << atan2(eMinusd(1), eMinusd(0)), circle_results.
qCharge * (tmp2 - fast_fit(2)),
399 circle_results.
qCharge * (1. / fast_fit(2) + uVec(
n));
404 jacobian << (radii(1, 0) * eMinusd(0) - eMinusd(1) * radii(0, 0)) * tmp1,
405 (radii(1, 1) * eMinusd(0) - eMinusd(1) * radii(0, 1)) * tmp1, 0,
406 circle_results.
qCharge * (eMinusd(0) * radii(0, 0) + eMinusd(1) * radii(1, 0)) * tmp2,
407 circle_results.
qCharge * (eMinusd(0) * radii(0, 1) + eMinusd(1) * radii(1, 1)) * tmp2, 0, 0, 0,
410 circle_results.
cov << iMat(0, 0), iMat(0, 1), iMat(0,
n), iMat(1, 0), iMat(1, 1), iMat(1,
n), iMat(
n, 0),
411 iMat(
n, 1), iMat(
n,
n);
413 circle_results.
cov = jacobian * circle_results.
cov * jacobian.transpose();
418 circle_results.
cov(0, 0) +=
426 circle_results.
chi2 = 0;
427 for (u_int
i = 0;
i <
n;
i++) {
429 if (
i > 0 &&
i <
n - 1)
430 circle_results.
chi2 +=
432 uVec(
i) * (sTransverse(
i + 1) - sTransverse(
i - 1)) /
433 ((sTransverse(
i + 1) - sTransverse(
i)) * (sTransverse(
i) - sTransverse(
i - 1))) +
434 uVec(
i + 1) / (sTransverse(
i + 1) - sTransverse(
i)) +
435 (sTransverse(
i + 1) - sTransverse(
i - 1)) * uVec(
n) / 2) /
462 template <
typename V4,
typename M6xN,
int n>
468 const auto& radii =
data.radii;
469 const auto& sTotal =
data.sTotal;
470 const auto& zInSZplane =
data.zInSZplane;
471 const auto& varBeta =
data.varBeta;
473 const double slope = -
data.qCharge / fast_fit(3);
478 riemannFit::Matrix2x3d::Zero();
480 for (u_int
i = 0;
i <
n;
i++) {
481 vMat(0, 0) = hits_ge.col(
i)[0];
482 vMat(0, 1) = vMat(1, 0) = hits_ge.col(
i)[1];
483 vMat(0, 2) = vMat(2, 0) = hits_ge.col(
i)[3];
484 vMat(1, 1) = hits_ge.col(
i)[2];
485 vMat(2, 1) = vMat(1, 2) = hits_ge.col(
i)[4];
486 vMat(2, 2) = hits_ge.col(
i)[5];
487 auto tmp = 1. / radii.block(0,
i, 2, 1).norm();
488 jacobXYZtosZ(0, 0) = radii(1,
i) *
tmp;
489 jacobXYZtosZ(0, 1) = -radii(0,
i) *
tmp;
490 jacobXYZtosZ(1, 2) = 1.;
491 weights(
i) = 1. / ((rotMat * jacobXYZtosZ * vMat * jacobXYZtosZ.transpose() * rotMat.transpose())(
496 for (u_int
i = 0;
i <
n;
i++) {
505 std::cout <<
"I4\n" << iMat << std::endl;
511 line_results.
par << (uVec(1) - uVec(0)) / (sTotal(1) - sTotal(0)), uVec(0);
512 auto idiff = 1. / (sTotal(1) - sTotal(0));
513 line_results.
cov << (iMat(0, 0) - 2 * iMat(0, 1) + iMat(1, 1)) *
riemannFit::sqr(idiff) +
515 (iMat(0, 1) - iMat(0, 0)) * idiff, (iMat(0, 1) - iMat(0, 0)) * idiff, iMat(0, 0);
521 jacobian(1, 0) = -sTotal(0);
523 line_results.
par(1) += -line_results.
par(0) * sTotal(0);
524 line_results.
cov = jacobian * line_results.
cov * jacobian.transpose();
527 auto tmp = rotMat(0, 0) - line_results.
par(0) * rotMat(0, 1);
528 jacobian(1, 1) = 1. /
tmp;
529 jacobian(0, 0) = jacobian(1, 1) * jacobian(1, 1);
531 jacobian(1, 0) = line_results.
par(1) * rotMat(0, 1) * jacobian(0, 0);
532 line_results.
par(1) = line_results.
par(1) * jacobian(1, 1);
533 line_results.
par(0) = (rotMat(0, 1) + line_results.
par(0) * rotMat(0, 0)) * jacobian(1, 1);
534 line_results.
cov = jacobian * line_results.
cov * jacobian.transpose();
537 line_results.
chi2 = 0;
538 for (u_int
i = 0;
i <
n;
i++) {
540 if (
i > 0 &&
i <
n - 1)
542 uVec(
i) * (sTotal(
i + 1) - sTotal(
i - 1)) /
543 ((sTotal(
i + 1) - sTotal(
i)) * (sTotal(
i) - sTotal(
i - 1))) +
544 uVec(
i + 1) / (sTotal(
i + 1) - sTotal(
i))) /
585 const Eigen::Matrix<float, 6, 4>& hits_ge,
601 jacobian << 1., 0, 0, 0, 1., 0, 0, 0,
604 circle.
cov = jacobian * circle.
cov * jacobian.transpose();
607 helix.
cov = riemannFit::MatrixXd::Zero(5, 5);
608 helix.
cov.block(0, 0, 3, 3) = circle.
cov;
609 helix.
cov.block(3, 3, 2, 2) =
line.cov;
619 #endif // RecoPixelVertexing_PixelTrackFitting_interface_BrokenLine_h
__host__ __device__ void lineFit(const M6xN &hits_ge, const V4 &fast_fit, const double bField, const PreparedBrokenLineData< n > &data, riemannFit::LineFit &line_results)
Performs the Broken Line fit in the straight track case (that is, the fit parameters are only the int...
int qCharge
particle charge
Eigen::Matrix< double, 2, N > Matrix2xNd
int32_t qCharge
particle charge
static const double slope[3]
Vector5d par
(phi,Tip,pt,cotan(theta)),Zip)
Sin< T >::type sin(const T &t)
Eigen::Matrix< double, N+1, 1 > VectorNplusONEd
Vector3d par
parameter: (X0,Y0,R)
__host__ __device__ double cross2D(const Vector2d &a, const Vector2d &b)
Compute cross product of two 2D vector (assuming z component 0), returning z component of the result...
constexpr std::array< uint8_t, layerIndexSize > layer
__host__ __device__ void translateKarimaki(karimaki_circle_fit &circle, double x0, double y0, riemannFit::Matrix3d &jacobian)
Changes the Karimäki parameters (and consequently their covariance matrix) under a translation of the...
__host__ __device__ void prepareBrokenLineData(const M3xN &hits, const V4 &fast_fit, const double bField, PreparedBrokenLineData< n > &results)
Computes the data needed for the Broken Line fit procedure that are mainly common for the circle and ...
Eigen::Matrix< double, 2, 3 > Matrix2x3d
__host__ __device__ double multScatt(const double &length, const double bField, const double radius, int layer, double slope)
Computes the Coulomb multiple scattering variance of the planar angle.
Eigen::Matrix< double, N, 1 > VectorNd
riemannFit::VectorNd< n > sTransverse
total distance traveled in the transverse plane
int32_t qCharge
particle charge
Cos< T >::type cos(const T &t)
Eigen::Matrix< double, N, N > MatrixNd
constexpr T sqr(const T a)
raise to square.
__host__ __device__ void fastFit(const M3xN &hits, V4 &result)
A very fast helix fit.
Abs< T >::type abs(const T &t)
riemannFit::HelixFit helixFit(const riemannFit::Matrix3xNd< n > &hits, const Eigen::Matrix< float, 6, 4 > &hits_ge, const double bField)
Helix fit by three step: -fast pre-fit (see Fast_fit() for further info); -circle fit of the hits pr...
Eigen::Matrix< double, 3, N > Matrix3xNd
__host__ __device__ riemannFit::Matrix2d rotationMatrix(double slope)
Computes the 2D rotation matrix that transforms the line y=slope*x into the line y=0.
__host__ __device__ riemannFit::MatrixNd< n > matrixC_u(const riemannFit::VectorNd< n > &weights, const riemannFit::VectorNd< n > &sTotal, const riemannFit::VectorNd< n > &varBeta)
Computes the n-by-n band matrix obtained minimizing the Broken Line's cost function w...
riemannFit::Matrix2xNd< n > radii
xy data in the system in which the pre-fitted center is the origin
riemannFit::VectorNd< n > zInSZplane
orthogonal coordinate to the pre-fitted line in the sz plane
Eigen::Matrix< double, N+1, N+1 > MatrixNplusONEd
char data[epos_bytes_allocation]
Vector2d par
(cotan(theta),Zip)
riemannFit::VectorNd< n > varBeta
kink angles in the SZ plane
static constexpr float d1
__host__ __device__ void circleFit(const M3xN &hits, const M6xN &hits_ge, const V4 &fast_fit, const double bField, PreparedBrokenLineData< n > &data, karimaki_circle_fit &circle_results)
Performs the Broken Line fit in the curved track case (that is, the fit parameters are the intercepti...
riemannFit::VectorNd< n > sTotal
total distance traveled (three-dimensional)
data needed for the Broken Line fit procedure.
double scalar(const CLHEP::HepGenMatrix &m)
Return the matrix as a scalar. Raise an assertion if the matris is not .