TkSeedGenerator/interface/FastCircleFit.h

0001 #ifndef RecoTracker_TkSeedGenerator_FastCircleFit_h
0002 #define RecoTracker_TkSeedGenerator_FastCircleFit_h
0003
0004 #include "DataFormats/GeometryVector/interface/GlobalPoint.h"
0005 #include "DataFormats/GeometryCommonDetAlgo/interface/GlobalError.h"
0006 #include "CommonTools/Utils/interface/DynArray.h"
0007
0008 #include <Eigen/Dense>
0009
0010 #ifdef MK_DEBUG
0011 #include <iostream>
0012 #define PRINT std::cout
0013 #else
0014 #include "FWCore/MessageLogger/interface/MessageLogger.h"
0015 #define PRINT LogTrace("FastCircleFit")
0016 #endif
0017
0018 #include <numeric>
0019 #include <vector>
0020
0021 /**
0022  * Same (almost) as FastCircle but with arbitrary number of hits.
0023  *
0024  * Strandlie, Wroldsen, Frühwirth NIM A 488 (2002) 332-341.
0025  * Frühwirth, Strandlie, Waltenberger NIM A 490 (2002) 366-378.
0026  */
0027 class FastCircleFit {
0028 public:
0029   /**
0030    * Constructor for containers of GlobalPoint and GlobalError
0031    *
0032    * @tparam P  Container of GlobalPoint
0033    * @tparam E  Container of GlobalError
0034    *
0035    * Container can be e.g. std::vector or DynArray.
0036    */
0037   template <typename P, typename E>
0038   FastCircleFit(const P& points, const E& errors) {
0039     const auto N = points.size();
0040     declareDynArray(float, N, x);
0041     declareDynArray(float, N, y);
0042     declareDynArray(float, N, z);
0043     declareDynArray(float, N, weight);  // 1/sigma^2
0044     calculate(points, errors, x, y, z, weight);
0045   }
0046
0047   /**
0048    * Constructor for std::array of GlobalPoint and GlobalError
0049    */
0050   template <size_t N>
0051   FastCircleFit(const std::array<GlobalPoint, N>& points, const std::array<GlobalError, N>& errors) {
0052     std::array<float, N> x;
0053     std::array<float, N> y;
0054     std::array<float, N> z;
0055     std::array<float, N> weight;  // 1/sigma^2
0056     calculate(points, errors, x, y, z, weight);
0057   }
0058
0059   ~FastCircleFit() = default;
0060
0061   float x0() const { return x0_; }
0062   float y0() const { return y0_; }
0063   float rho() const { return rho_; }
0064
0065   // TODO: I'm not sure if the minimized square sum is chi2 distributed
0066   float chi2() const { return chi2_; }
0067
0068 private:
0069   template <typename P, typename E, typename C>
0070   void calculate(const P& points, const E& errors, C& x, C& y, C& z, C& weight);
0071
0072   template <typename T>
0073   T sqr(T t) {
0074     return t * t;
0075   }
0076
0077   float x0_;
0078   float y0_;
0079   float rho_;
0080   float chi2_;
0081 };
0082
0083 template <typename P, typename E, typename C>
0084 void FastCircleFit::calculate(const P& points, const E& errors, C& x, C& y, C& z, C& weight) {
0085   const auto N = points.size();
0086
0087   // transform
0088   for (size_t i = 0; i < N; ++i) {
0089     const auto& point = points[i];
0090     const auto& p = point.basicVector();
0091     x[i] = p.x();
0092     y[i] = p.y();
0093     z[i] = sqr(p.x()) + sqr(p.y());
0094
0095     // TODO: The following accounts only the hit uncertainty in phi.
0096     // Albeit it "works nicely", should we account also the
0097     // uncertainty in r in FPix (and the correlation between phi and r)?
0098     const float phiErr2 = errors[i].phierr(point);
0099     const float w = 1.f / (point.perp2() * phiErr2);
0100     weight[i] = w;
0101
0102     PRINT << " point " << p.x() << " " << p.y() << " transformed " << x[i] << " " << y[i] << " " << z[i] << " weight "
0103           << w << std::endl;
0104   }
0105   const float invTotWeight = 1.f / std::accumulate(weight.begin(), weight.end(), 0.f);
0106   PRINT << " invTotWeight " << invTotWeight;
0107
0108   Eigen::Vector3f mean = Eigen::Vector3f::Zero();
0109   for (size_t i = 0; i < N; ++i) {
0110     const float w = weight[i];
0111     mean[0] += w * x[i];
0112     mean[1] += w * y[i];
0113     mean[2] += w * z[i];
0114   }
0115   mean *= invTotWeight;
0116   PRINT << " mean " << mean[0] << " " << mean[1] << " " << mean[2] << std::endl;
0117
0118   Eigen::Matrix3f A = Eigen::Matrix3f::Zero();
0119   for (size_t i = 0; i < N; ++i) {
0120     const auto diff_x = x[i] - mean[0];
0121     const auto diff_y = y[i] - mean[1];
0122     const auto diff_z = z[i] - mean[2];
0123     const auto w = weight[i];
0124
0125     // exploit the fact that only lower triangular part of the matrix
0126     // is used in the eigenvalue calculation
0127     A(0, 0) += w * diff_x * diff_x;
0128     A(1, 0) += w * diff_y * diff_x;
0129     A(1, 1) += w * diff_y * diff_y;
0130     A(2, 0) += w * diff_z * diff_x;
0131     A(2, 1) += w * diff_z * diff_y;
0132     A(2, 2) += w * diff_z * diff_z;
0133     PRINT << " i " << i << " A " << A(0, 0) << " " << A(0, 1) << " " << A(0, 2) << std::endl
0134           << "     " << A(1, 0) << " " << A(1, 1) << " " << A(1, 2) << std::endl
0135           << "     " << A(2, 0) << " " << A(2, 1) << " " << A(2, 2) << std::endl;
0136   }
0137   A *= 1. / static_cast<float>(N);
0138
0139   PRINT << " A " << A(0, 0) << " " << A(0, 1) << " " << A(0, 2) << std::endl
0140         << "   " << A(1, 0) << " " << A(1, 1) << " " << A(1, 2) << std::endl
0141         << "   " << A(2, 0) << " " << A(2, 1) << " " << A(2, 2) << std::endl;
0142
0143   // find eigenvalues and vectors
0144   Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> eigen(A);
0145   const auto& eigenValues = eigen.eigenvalues();
0146   const auto& eigenVectors = eigen.eigenvectors();
0147
0148   // in Eigen the first eigenvalue is the smallest
0149   PRINT << " eigenvalues " << eigenValues[0] << " " << eigenValues[1] << " " << eigenValues[2] << std::endl;
0150
0151   PRINT << " eigen " << eigenVectors(0, 0) << " " << eigenVectors(0, 1) << " " << eigenVectors(0, 2) << std::endl
0152         << "       " << eigenVectors(1, 0) << " " << eigenVectors(1, 1) << " " << eigenVectors(1, 2) << std::endl
0153         << "       " << eigenVectors(2, 0) << " " << eigenVectors(2, 1) << " " << eigenVectors(2, 2) << std::endl;
0154
0155   // eivenvector corresponding smallest eigenvalue
0156   auto n = eigenVectors.col(0);
0157   PRINT << " n (unit) " << n[0] << " " << n[1] << " " << n[2] << std::endl;
0158
0159   const float c = -1.f * (n[0] * mean[0] + n[1] * mean[1] + n[2] * mean[2]);
0160
0161   PRINT << " c " << c << std::endl;
0162
0163   // calculate fit parameters
0164   const auto tmp = 0.5f * 1.f / n[2];
0165   x0_ = -n[0] * tmp;
0166   y0_ = -n[1] * tmp;
0167   const float rho2 = (1 - sqr(n[2]) - 4 * c * n[2]) * sqr(tmp);
0168   rho_ = std::sqrt(rho2);
0169
0170   // calculate square sum
0171   float S = 0;
0172   for (size_t i = 0; i < N; ++i) {
0173     const float incr = sqr(c + n[0] * x[i] + n[1] * y[i] + n[2] * z[i]) * weight[i];
0174 #if defined(MK_DEBUG) || defined(EDM_ML_DEBUG)
0175     const float distance = std::sqrt(sqr(x[i] - x0_) + sqr(y[i] - y0_)) - rho_;
0176     PRINT << " i " << i << " chi2 incr " << incr << " d(hit, circle) " << distance << " sigma "
0177           << 1.f / std::sqrt(weight[i]) << std::endl;
0178 #endif
0179     S += incr;
0180   }
0181   chi2_ = S;
0182 }
0183
0184 #endif