FastCircleFit.h

Go to the documentation of this file.

#ifndef RecoTracker_TkSeedGenerator_FastCircleFit_h
#define RecoTracker_TkSeedGenerator_FastCircleFit_h
 
#include "DataFormats/GeometryVector/interface/GlobalPoint.h"
#include "DataFormats/GeometryCommonDetAlgo/interface/GlobalError.h"
#include "CommonTools/Utils/interface/DynArray.h"
 
#include <Eigen/Dense>
 
#ifdef MK_DEBUG
#include <iostream>
#define PRINT std::cout
#else
#include "FWCore/MessageLogger/interface/MessageLogger.h"
#define PRINT LogTrace("FastCircleFit")
#endif
 
#include <numeric>
#include <vector>
 
class FastCircleFit {
public:
  template <typename P, typename E>
  FastCircleFit(const P& points, const E& errors) {
    const auto N = points.size();
    declareDynArray(float, N, x);
    declareDynArray(float, N, y);
    declareDynArray(float, N, z);
    declareDynArray(float, N, weight);  // 1/sigma^2
    calculate(points, errors, x, y, z, weight);
  }
 
  template <size_t N>
  FastCircleFit(const std::array<GlobalPoint, N>& points, const std::array<GlobalError, N>& errors) {
    std::array<float, N> x;
    std::array<float, N> y;
    std::array<float, N> z;
    std::array<float, N> weight;  // 1/sigma^2
    calculate(points, errors, x, y, z, weight);
  }
 
  ~FastCircleFit() = default;
 
  float x0() const { return x0_; }
  float y0() const { return y0_; }
  float rho() const { return rho_; }
 
  // TODO: I'm not sure if the minimized square sum is chi2 distributed
  float chi2() const { return chi2_; }
 
private:
  template <typename P, typename E, typename C>
  void calculate(const P& points, const E& errors, C& x, C& y, C& z, C& weight);
 
  template <typename T>
  T sqr(T t) {
    return t * t;
  }
 
  float x0_;
  float y0_;
  float rho_;
  float chi2_;
};
 
template <typename P, typename E, typename C>
void FastCircleFit::calculate(const P& points, const E& errors, C& x, C& y, C& z, C& weight) {
  const auto N = points.size();
 
  // transform
  for (size_t i = 0; i < N; ++i) {
    const auto& point = points[i];
    const auto& p = point.basicVector();
    x[i] = p.x();
    y[i] = p.y();
    z[i] = sqr(p.x()) + sqr(p.y());
 
    // TODO: The following accounts only the hit uncertainty in phi.
    // Albeit it "works nicely", should we account also the
    // uncertainty in r in FPix (and the correlation between phi and r)?
    const float phiErr2 = errors[i].phierr(point);
    const float w = 1.f / (point.perp2() * phiErr2);
    weight[i] = w;
 
    PRINT << " point " << p.x() << " " << p.y() << " transformed " << x[i] << " " << y[i] << " " << z[i] << " weight "
          << w << std::endl;
  }
  const float invTotWeight = 1.f / std::accumulate(weight.begin(), weight.end(), 0.f);
  PRINT << " invTotWeight " << invTotWeight;
 
  Eigen::Vector3f mean = Eigen::Vector3f::Zero();
  for (size_t i = 0; i < N; ++i) {
    const float w = weight[i];
    mean[0] += w * x[i];
    mean[1] += w * y[i];
    mean[2] += w * z[i];
  }
  mean *= invTotWeight;
  PRINT << " mean " << mean[0] << " " << mean[1] << " " << mean[2] << std::endl;
 
  Eigen::Matrix3f A = Eigen::Matrix3f::Zero();
  for (size_t i = 0; i < N; ++i) {
    const auto diff_x = x[i] - mean[0];
    const auto diff_y = y[i] - mean[1];
    const auto diff_z = z[i] - mean[2];
    const auto w = weight[i];
 
    // exploit the fact that only lower triangular part of the matrix
    // is used in the eigenvalue calculation
    A(0, 0) += w * diff_x * diff_x;
    A(1, 0) += w * diff_y * diff_x;
    A(1, 1) += w * diff_y * diff_y;
    A(2, 0) += w * diff_z * diff_x;
    A(2, 1) += w * diff_z * diff_y;
    A(2, 2) += w * diff_z * diff_z;
    PRINT << " i " << i << " A " << A(0, 0) << " " << A(0, 1) << " " << A(0, 2) << std::endl
          << "     " << A(1, 0) << " " << A(1, 1) << " " << A(1, 2) << std::endl
          << "     " << A(2, 0) << " " << A(2, 1) << " " << A(2, 2) << std::endl;
  }
  A *= 1. / static_cast<float>(N);
 
  PRINT << " A " << A(0, 0) << " " << A(0, 1) << " " << A(0, 2) << std::endl
        << "   " << A(1, 0) << " " << A(1, 1) << " " << A(1, 2) << std::endl
        << "   " << A(2, 0) << " " << A(2, 1) << " " << A(2, 2) << std::endl;
 
  // find eigenvalues and vectors
  Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> eigen(A);
  const auto& eigenValues = eigen.eigenvalues();
  const auto& eigenVectors = eigen.eigenvectors();
 
  // in Eigen the first eigenvalue is the smallest
  PRINT << " eigenvalues " << eigenValues[0] << " " << eigenValues[1] << " " << eigenValues[2] << std::endl;
 
  PRINT << " eigen " << eigenVectors(0, 0) << " " << eigenVectors(0, 1) << " " << eigenVectors(0, 2) << std::endl
        << "       " << eigenVectors(1, 0) << " " << eigenVectors(1, 1) << " " << eigenVectors(1, 2) << std::endl
        << "       " << eigenVectors(2, 0) << " " << eigenVectors(2, 1) << " " << eigenVectors(2, 2) << std::endl;
 
  // eivenvector corresponding smallest eigenvalue
  auto n = eigenVectors.col(0);
  PRINT << " n (unit) " << n[0] << " " << n[1] << " " << n[2] << std::endl;
 
  const float c = -1.f * (n[0] * mean[0] + n[1] * mean[1] + n[2] * mean[2]);
 
  PRINT << " c " << c << std::endl;
 
  // calculate fit parameters
  const auto tmp = 0.5f * 1.f / n[2];
  x0_ = -n[0] * tmp;
  y0_ = -n[1] * tmp;
  const float rho2 = (1 - sqr(n[2]) - 4 * c * n[2]) * sqr(tmp);
  rho_ = std::sqrt(rho2);
 
  // calculate square sum
  float S = 0;
  for (size_t i = 0; i < N; ++i) {
    const float incr = sqr(c + n[0] * x[i] + n[1] * y[i] + n[2] * z[i]) * weight[i];
#if defined(MK_DEBUG) || defined(EDM_ML_DEBUG)
    const float distance = std::sqrt(sqr(x[i] - x0_) + sqr(y[i] - y0_)) - rho_;
    PRINT << " i " << i << " chi2 incr " << incr << " d(hit, circle) " << distance << " sigma "
          << 1.f / std::sqrt(weight[i]) << std::endl;
#endif
    S += incr;
  }
  chi2_ = S;
}
 
#endif