File indexing completed on 2023-04-15 01:47:37
0001 #ifndef RecoTracker_PixelTrackFitting_FitUtils_h
0002 #define RecoTracker_PixelTrackFitting_FitUtils_h
0003
0004 #include "DataFormats/Math/interface/choleskyInversion.h"
0005 #include "HeterogeneousCore/CUDAUtilities/interface/cuda_assert.h"
0006 #include "RecoTracker/PixelTrackFitting/interface/FitResult.h"
0007
0008 namespace riemannFit {
0009
0010 constexpr double epsilon = 1.e-4;
0011
0012 using VectorXd = Eigen::VectorXd;
0013 using MatrixXd = Eigen::MatrixXd;
0014 template <int N>
0015 using MatrixNd = Eigen::Matrix<double, N, N>;
0016 template <int N>
0017 using MatrixNplusONEd = Eigen::Matrix<double, N + 1, N + 1>;
0018 template <int N>
0019 using ArrayNd = Eigen::Array<double, N, N>;
0020 template <int N>
0021 using Matrix2Nd = Eigen::Matrix<double, 2 * N, 2 * N>;
0022 template <int N>
0023 using Matrix3Nd = Eigen::Matrix<double, 3 * N, 3 * N>;
0024 template <int N>
0025 using Matrix2xNd = Eigen::Matrix<double, 2, N>;
0026 template <int N>
0027 using Array2xNd = Eigen::Array<double, 2, N>;
0028 template <int N>
0029 using MatrixNx3d = Eigen::Matrix<double, N, 3>;
0030 template <int N>
0031 using MatrixNx5d = Eigen::Matrix<double, N, 5>;
0032 template <int N>
0033 using VectorNd = Eigen::Matrix<double, N, 1>;
0034 template <int N>
0035 using VectorNplusONEd = Eigen::Matrix<double, N + 1, 1>;
0036 template <int N>
0037 using Vector2Nd = Eigen::Matrix<double, 2 * N, 1>;
0038 template <int N>
0039 using Vector3Nd = Eigen::Matrix<double, 3 * N, 1>;
0040 template <int N>
0041 using RowVectorNd = Eigen::Matrix<double, 1, 1, N>;
0042 template <int N>
0043 using RowVector2Nd = Eigen::Matrix<double, 1, 2 * N>;
0044
0045 using Matrix2x3d = Eigen::Matrix<double, 2, 3>;
0046
0047 using Matrix3f = Eigen::Matrix3f;
0048 using Vector3f = Eigen::Vector3f;
0049 using Vector4f = Eigen::Vector4f;
0050 using Vector6f = Eigen::Matrix<double, 6, 1>;
0051
0052 template <class C>
0053 __host__ __device__ void printIt(C* m, const char* prefix = "") {
0054 #ifdef RFIT_DEBUG
0055 for (uint r = 0; r < m->rows(); ++r) {
0056 for (uint c = 0; c < m->cols(); ++c) {
0057 printf("%s Matrix(%d,%d) = %g\n", prefix, r, c, (*m)(r, c));
0058 }
0059 }
0060 #endif
0061 }
0062
0063
0064
0065
0066 template <typename T>
0067 constexpr T sqr(const T a) {
0068 return a * a;
0069 }
0070
0071
0072
0073
0074
0075
0076
0077
0078
0079 __host__ __device__ inline double cross2D(const Vector2d& a, const Vector2d& b) {
0080 return a.x() * b.y() - a.y() * b.x();
0081 }
0082
0083
0084
0085
0086
0087 template <typename M6xNf, typename M2Nd>
0088 __host__ __device__ void loadCovariance2D(M6xNf const& ge, M2Nd& hits_cov) {
0089
0090
0091
0092
0093
0094
0095
0096
0097
0098
0099
0100
0101 constexpr uint32_t hits_in_fit = M6xNf::ColsAtCompileTime;
0102 for (uint32_t i = 0; i < hits_in_fit; ++i) {
0103 {
0104 constexpr uint32_t ge_idx = 0, j = 0, l = 0;
0105 hits_cov(i + j * hits_in_fit, i + l * hits_in_fit) = ge.col(i)[ge_idx];
0106 }
0107 {
0108 constexpr uint32_t ge_idx = 2, j = 1, l = 1;
0109 hits_cov(i + j * hits_in_fit, i + l * hits_in_fit) = ge.col(i)[ge_idx];
0110 }
0111 {
0112 constexpr uint32_t ge_idx = 1, j = 1, l = 0;
0113 hits_cov(i + l * hits_in_fit, i + j * hits_in_fit) = hits_cov(i + j * hits_in_fit, i + l * hits_in_fit) =
0114 ge.col(i)[ge_idx];
0115 }
0116 }
0117 }
0118
0119 template <typename M6xNf, typename M3xNd>
0120 __host__ __device__ void loadCovariance(M6xNf const& ge, M3xNd& hits_cov) {
0121
0122
0123
0124
0125
0126
0127
0128
0129
0130
0131
0132
0133 constexpr uint32_t hits_in_fit = M6xNf::ColsAtCompileTime;
0134 for (uint32_t i = 0; i < hits_in_fit; ++i) {
0135 {
0136 constexpr uint32_t ge_idx = 0, j = 0, l = 0;
0137 hits_cov(i + j * hits_in_fit, i + l * hits_in_fit) = ge.col(i)[ge_idx];
0138 }
0139 {
0140 constexpr uint32_t ge_idx = 2, j = 1, l = 1;
0141 hits_cov(i + j * hits_in_fit, i + l * hits_in_fit) = ge.col(i)[ge_idx];
0142 }
0143 {
0144 constexpr uint32_t ge_idx = 5, j = 2, l = 2;
0145 hits_cov(i + j * hits_in_fit, i + l * hits_in_fit) = ge.col(i)[ge_idx];
0146 }
0147 {
0148 constexpr uint32_t ge_idx = 1, j = 1, l = 0;
0149 hits_cov(i + l * hits_in_fit, i + j * hits_in_fit) = hits_cov(i + j * hits_in_fit, i + l * hits_in_fit) =
0150 ge.col(i)[ge_idx];
0151 }
0152 {
0153 constexpr uint32_t ge_idx = 3, j = 2, l = 0;
0154 hits_cov(i + l * hits_in_fit, i + j * hits_in_fit) = hits_cov(i + j * hits_in_fit, i + l * hits_in_fit) =
0155 ge.col(i)[ge_idx];
0156 }
0157 {
0158 constexpr uint32_t ge_idx = 4, j = 2, l = 1;
0159 hits_cov(i + l * hits_in_fit, i + j * hits_in_fit) = hits_cov(i + j * hits_in_fit, i + l * hits_in_fit) =
0160 ge.col(i)[ge_idx];
0161 }
0162 }
0163 }
0164
0165
0166
0167
0168
0169
0170
0171
0172
0173 __host__ __device__ inline void par_uvrtopak(CircleFit& circle, const double B, const bool error) {
0174 Vector3d par_pak;
0175 const double temp0 = circle.par.head(2).squaredNorm();
0176 const double temp1 = sqrt(temp0);
0177 par_pak << atan2(circle.qCharge * circle.par(0), -circle.qCharge * circle.par(1)),
0178 circle.qCharge * (temp1 - circle.par(2)), circle.par(2) * B;
0179 if (error) {
0180 const double temp2 = sqr(circle.par(0)) * 1. / temp0;
0181 const double temp3 = 1. / temp1 * circle.qCharge;
0182 Matrix3d j4Mat;
0183 j4Mat << -circle.par(1) * temp2 * 1. / sqr(circle.par(0)), temp2 * 1. / circle.par(0), 0., circle.par(0) * temp3,
0184 circle.par(1) * temp3, -circle.qCharge, 0., 0., B;
0185 circle.cov = j4Mat * circle.cov * j4Mat.transpose();
0186 }
0187 circle.par = par_pak;
0188 }
0189
0190
0191
0192
0193
0194
0195
0196 __host__ __device__ inline void fromCircleToPerigee(CircleFit& circle) {
0197 Vector3d par_pak;
0198 const double temp0 = circle.par.head(2).squaredNorm();
0199 const double temp1 = sqrt(temp0);
0200 par_pak << atan2(circle.qCharge * circle.par(0), -circle.qCharge * circle.par(1)),
0201 circle.qCharge * (temp1 - circle.par(2)), circle.qCharge / circle.par(2);
0202
0203 const double temp2 = sqr(circle.par(0)) * 1. / temp0;
0204 const double temp3 = 1. / temp1 * circle.qCharge;
0205 Matrix3d j4Mat;
0206 j4Mat << -circle.par(1) * temp2 * 1. / sqr(circle.par(0)), temp2 * 1. / circle.par(0), 0., circle.par(0) * temp3,
0207 circle.par(1) * temp3, -circle.qCharge, 0., 0., -circle.qCharge / (circle.par(2) * circle.par(2));
0208 circle.cov = j4Mat * circle.cov * j4Mat.transpose();
0209
0210 circle.par = par_pak;
0211 }
0212
0213
0214
0215
0216
0217 template <typename VI5, typename MI5, typename VO5, typename MO5>
0218 __host__ __device__ inline void transformToPerigeePlane(VI5 const& ip, MI5 const& icov, VO5& op, MO5& ocov) {
0219 auto sinTheta2 = 1. / (1. + ip(3) * ip(3));
0220 auto sinTheta = std::sqrt(sinTheta2);
0221 auto cosTheta = ip(3) * sinTheta;
0222
0223 op(0) = sinTheta * ip(2);
0224 op(1) = 0.;
0225 op(2) = -ip(3);
0226 op(3) = ip(1);
0227 op(4) = -ip(4);
0228
0229 Matrix5d jMat = Matrix5d::Zero();
0230
0231 jMat(0, 2) = sinTheta;
0232 jMat(0, 3) = -sinTheta2 * cosTheta * ip(2);
0233 jMat(1, 0) = 1.;
0234 jMat(2, 3) = -1.;
0235 jMat(3, 1) = 1.;
0236 jMat(4, 4) = -1;
0237
0238 ocov = jMat * icov * jMat.transpose();
0239 }
0240
0241 }
0242
0243 #endif