SdPlaneFit.cc

Go to the documentation of this file.

#include <cmath>

#include <fwk/CentralConfig.h>
#include <fwk/RunController.h>
#include <fwk/LocalCoordinateSystem.h>

#include <det/Detector.h>

#include <sdet/SDetector.h>
#include <sdet/STimeVariance.h>

#include <evt/Event.h>
#include <evt/ShowerRecData.h>
#include <evt/ShowerSRecData.h>
#include <evt/ShowerSimData.h>

#include <sevt/SEvent.h>
#include <sevt/EventTrigger.h>
#include <sevt/Station.h>
#include <sevt/StationRecData.h>

#include <utl/Reader.h>
#include <utl/ErrorLogger.h>
#include <utl/PhysicalConstants.h>
#include <utl/AxialVector.h>
#include <utl/Math.h>
#include <utl/String.h>
#include <utl/config.h>

#include <CLHEP/Matrix/Matrix.h>
#include <CLHEP/Matrix/Vector.h>

#include <TMinuit.h>

#include "SdPlaneFit.h"

using namespace SdPlaneFitOG;
using namespace fwk;
using namespace evt;
using namespace sevt;
using namespace utl;
using namespace std;

using CLHEP::HepMatrix;
using CLHEP::HepVector;


#define OUT(_x_) if ((_x_) <= fInfoLevel) cerr


// global variables
SEvent* SdPlaneFit::fgCurrentSEvent = nullptr;
Point SdPlaneFit::fgBarycenter;
TimeStamp SdPlaneFit::fgBaryTime;
CoordinateSystemPtr SdPlaneFit::fgBarycenterCS;


namespace SdPlaneFitOG {

  // square of the perpendicular distance from the axis
  inline
  double
  RPerp2(const Vector& axis, const Vector& station)
  {
    const double scal = axis * station;
    return station*station - scal*scal;
  }


  VModule::ResultFlag
  SdPlaneFit::Init()
  {
    const auto topB = CentralConfig::GetInstance()->GetTopBranch("SdPlaneFit");

    topB.GetChild("infoLevel").GetData(fInfoLevel);
    if (fInfoLevel < eNone || fInfoLevel > eMinuit) {
      INFO("<infoLevel> out of bounds");
      return eFailure;
    }
    fMinuitOutput = (fInfoLevel >= eMinuit);

    topB.GetChild("minNumberOfStations").GetData(fMinNumberOfStations);
    if (fMinNumberOfStations <= 2) {
      INFO("<minNumberOfStations> must be > 2");
      return eFailure;
    }

    return eSuccess;
  }


  // analytic linear fit in local CS; approximation by neglecting
  // z-coordinate, since z ~ 0 in local bary coordinate system,
  // therefore losing the w component of the axis. it is funny
  // though, that stations living on a plane can not distinguish
  // between upwards or downwards-going showers.

  VModule::ResultFlag
  SdPlaneFit::LinearFit(FitParameters& fit, const bool reuseFit)
    const
  {
    const double cosTheta = (reuseFit ? fit.w : 1);
    const Vector approxAxis =
      (reuseFit ? Vector(fit.u, fit.v, fit.w, fgBarycenterCS) : Vector());
    const auto& sDetector = det::Detector::GetInstance().GetSDetector();
    const auto& timeVariance = sdet::STimeVariance::GetInstance();

    double s1 = 0;
    double sx = 0;
    double sy = 0;
    double st = 0;
    double sxx = 0;
    double sxy = 0;
    double syy = 0;
    double sxt = 0;
    double syt = 0;
    //double stt = 0;

    for (const auto& s : fgCurrentSEvent->CandidateStationsRange()) {
      const Vector sPos = sDetector.GetStation(s).GetPosition() - fgBarycenter;
      const double x = sPos.GetX(fgBarycenterCS);
      const double y = sPos.GetY(fgBarycenterCS);
      const auto& sRec = s.GetRecData();
      const double t = (sRec.GetSignalStartTime() - fgBaryTime).GetInterval();
      const double sigma2 =
        timeVariance.GetTimeSigma2(sRec.GetTotalSignal(), sRec.GetT50(), cosTheta, sqrt(RPerp2(approxAxis, sPos))) +
        sRec.GetGPSTimeVariance();
      const double invSigma2 = 1 / sigma2;
      s1 += invSigma2;
      const double x_s = x * invSigma2;
      sx += x_s;
      sxx += x*x_s;
      sxy += y*x_s;
      sxt += t*x_s;
      const double y_s = y * invSigma2;
      sy += y_s;
      syy += y*y_s;
      syt += t*y_s;
      st += t * invSigma2;
      //stt += t*t;
    }

    st *= kSpeedOfLight;
    sxt *= kSpeedOfLight;
    syt *= kSpeedOfLight;
    //stt *= kSpeedOfLight*kSpeedOfLight;

    HepMatrix mat(3, 3);

    mat[0][0] = sxx; mat[0][1] = sxy; mat[0][2] = -sx;
    mat[1][0] = sxy; mat[1][1] = syy; mat[1][2] = -sy;
    mat[2][0] = -sx; mat[2][1] = -sy; mat[2][2] = s1;

    int ierr = 0;
    mat.invert(ierr);

    if (ierr) {
      INFO("Matrix inversion failed.");
      return eFailure;
    }

    HepVector k(3);

    k[0] = -sxt;
    k[1] = -syt;
    k[2] = st;

    const HepVector p = mat*k;

    const double w2 = 1 - Sqr(p[0]) - Sqr(p[1]);

    if (w2 < 0) {
      INFO("Unphysical direction cosines");
      return eFailure;
    }

    fit.u = p[0];
    fit.v = p[1];
    fit.w = sqrt(w2);
    fit.ct0 = p[2];

    fit.sigmaU2 = mat[0][0];
    fit.sigmaV2 = mat[1][1];
    fit.sigmaUV = mat[0][1];
    fit.sigmact02 = mat[2][2];

    fit.stage = (reuseFit ? ShowerSRecData::kPlaneFitLinear2 : ShowerSRecData::kPlaneFitLinear);

    return eSuccess;
  }


  VModule::ResultFlag
  SdPlaneFit::Run(evt::Event& event)
  {
    // nothing to do if there is no SEvent
    if (!event.HasSEvent())
      return eContinueLoop;

    fgCurrentSEvent = &event.GetSEvent();
    const auto& sEvent = *fgCurrentSEvent;

    const auto& detector = det::Detector::GetInstance();
    const auto& sDetector = detector.GetSDetector();

    // calculate barycenter and bary-time and determine the # of relevant stations

    const auto& siteCS = detector.GetSiteCoordinateSystem();

    // the absolute points in time and space
    const Point siteOrigin(0,0,0, siteCS);

    if (!event.GetSEvent().HasTrigger())
      return eContinueLoop;

    const auto& eventTime = event.GetSEvent().GetTrigger().GetTime();

    // weighted sums
    Vector barySum(0,0,0, siteCS);
    TimeInterval timeSum = 0;
    double weightSum = 0;
    int nStations = 0;
    for (const auto& s : sEvent.CandidateStationsRange()) {
      const auto& dStation = sDetector.GetStation(s);
      const auto& sRec = s.GetRecData();
      const double weight = sqrt(sRec.GetTotalSignal());
      weightSum += weight;
      barySum += weight * (dStation.GetPosition() - siteOrigin);
      timeSum += weight * (sRec.GetSignalStartTime() - eventTime);
      ++nStations;
    }

    OUT(eIntermediate)
      << "# candidate stations = " << nStations << endl;

    // not worth it
    if (nStations < fMinNumberOfStations) {
      OUT(eFinal)
        << "Not enough stations." << endl;
      return eContinueLoop;
    }

    if (!weightSum) {
      OUT(eObscure)
        << "sum of weights = 0" << endl;
      return eContinueLoop;
    }

    barySum /= weightSum;
    timeSum /= weightSum;
    fgBarycenter = siteOrigin + barySum;
    fgBaryTime = eventTime + timeSum;
    fgBarycenterCS = LocalCoordinateSystem::Create(fgBarycenter);

    OUT(eIntermediate)
      << "barycenter = (" << String::Make(fgBarycenter, siteCS, meter, ", ") << ") m (site)\n"
         "bary time = " << timeSum/nanosecond << " ns (to event time)\n";

    OUT(eObscure)
      << "local/site zenith angle diff = "
      << Angle(Vector(0,0,1, fgBarycenterCS), Vector(0,0,1, siteCS))/degree
      << " deg\n";

    if (event.HasSimShower()) {
      const auto mcAxis = -event.GetSimShower().GetDirection();
      OUT(eIntermediate)
        << "MC axis = (" << String::Make(mcAxis, fgBarycenterCS, 1, ", ") << ") (bary)\n";
    }

    // this will point to the final result
    FitParameters* fit = nullptr;

    // first approximation
    FitParameters fitLin;
    if (LinearFit(fitLin) != eSuccess)
      return eContinueLoop;
    // try again with previous values as approxiamtions for sigma
    LinearFit(fitLin, true);

    // with first approximation on u and v, the nonlinear fit is attempted
    FitParameters fitNonlin = fitLin;
    if (PlaneFit3DDriver(fitNonlin) == eSuccess) {
      //fitNonlin.stage = ShowerSRecData::kPlaneFit3d;
      OUT(eIntermediate)
        << "Stage " << fitLin.stage << ": linear plane fit\n"
           "  axis = (" << fitLin.u << ", " << fitLin.v << ", " << fitLin.w << ") (bary)\n"
           "  ct0 = " << fitLin.ct0/meter << " m" << endl;
      fit = &fitNonlin;
    } else {
      OUT(eIntermediate)
        << "  Nonlinear fit failed, using linear approximation." << endl;
      fit = &fitLin;
    }

    // Fill the event
    if (!event.HasRecShower())
      event.MakeRecShower();

    if (!event.GetRecShower().HasSRecShower())
      event.GetRecShower().MakeSRecShower();

    auto& showerSRec = event.GetRecShower().GetSRecShower();

    showerSRec.SetBarycenter(fgBarycenter);
    showerSRec.SetBarytime(fgBaryTime);

    const Vector axis(fit->u, fit->v, fit->w, fgBarycenterCS);
    showerSRec.SetAxis(axis);

    showerSRec.SetAngleErrors(axis.GetCoordinates(fgBarycenterCS),
                              Vector::Triple(fit->sigmaU2, fit->sigmaUV, fit->sigmaV2));

    showerSRec.SetCurvature(0, 0);
    showerSRec.SetCorePosition(fgBarycenter);
    showerSRec.SetCoreTime(fgBaryTime + TimeInterval(fit->ct0/kSpeedOfLight),
                           TimeInterval(sqrt(fit->sigmact02)));

    double timeMean3d = 0;
    double timeSpread3d = 0;
    double chi2 = 0;
    CalculateTimeResidual3D(axis, fit->ct0, timeMean3d, timeSpread3d, chi2);

    showerSRec.SetTimeResidualMean(timeMean3d);
    showerSRec.SetTimeResidualSpread(timeSpread3d);
    const int angleNDOF = nStations - 3;
    showerSRec.SetAngleChi2(chi2, angleNDOF);

    showerSRec.SetLDFRecStage(fit->stage);

    OUT(eFinal)
      << "Stage " << fit->stage << ": "
      << (fit == &fitLin ? "linear" : "3d" )
      << "  plane fit\n"
         "  axis = (" << fit->u << ", " << fit->v << ", " << fit->w << ") (bary)\n"
         "  theta = " << acos(fit->w)/degree << " +/- "
                      << showerSRec.GetThetaError()/degree << " deg (bary)\n"
         "  phi = " << atan2(fit->v, fit->u)/degree << " +/- "
                    << showerSRec.GetPhiError()/degree << " deg\n"
         "  ct0 = " << fit->ct0/meter << " m\n"
         "  time chi2 = " << chi2 << " / " << angleNDOF << "\n"
         "  time residual = " << timeMean3d << " +/- "
                              << timeSpread3d << endl;
    if (fit != &fitLin)
      OUT(eFinal)
        << "  axis diff = " << Angle(Vector(fitLin.u, fitLin.v, fitLin.w, fgBarycenterCS), axis)/degree
                            << " deg (to stage " << fitLin.stage << ")\n";

    // fill the new structure for plane-front reconstruction
    if (!showerSRec.HasPlaneFrontRecData())
      showerSRec.MakePlaneFrontRecData();
    auto& planeFrontRec = showerSRec.GetPlaneFrontRecData();

    planeFrontRec.SetBarycenter(fgBarycenter);
    planeFrontRec.SetBarycenterTime(fgBaryTime + TimeInterval(fit->ct0/kSpeedOfLight));
    planeFrontRec.SetAxis(axis);
    planeFrontRec.SetAngleErrors(axis.GetCoordinates(fgBarycenterCS),
                                 Vector::Triple(fit->sigmaU2, fit->sigmaUV, fit->sigmaV2));
    planeFrontRec.SetAngleChi2Ndof(chi2, angleNDOF);
    planeFrontRec.SetTimeResidualMean(timeMean3d, timeSpread3d);

    ++RunController::GetInstance().GetRunData().GetNamedCounters()["SdPlaneFit/Axis"];

    return eSuccess;
  }


  void
  SdPlaneFit::CalculateTimeResidual3D(const Vector& axis, const double ct0,
                                      double& mean, double& spread, double& chi2)
    const
  {
    const Vector axis_c = axis / kSpeedOfLight;
    const double t0 = ct0 / kSpeedOfLight;

    const auto& sDetector = det::Detector::GetInstance().GetSDetector();
    const auto& timeVariance = sdet::STimeVariance::GetInstance();

    const double cosTheta = axis.GetCosTheta(fgBarycenterCS);

    double sum = 0;
    chi2 = 0;
    int n = 0;
    for (auto& s : fgCurrentSEvent->CandidateStationsRange()) {

      const Vector vx = sDetector.GetStation(s).GetPosition() - fgBarycenter;
      const double t = (s.GetRecData().GetSignalStartTime() - fgBaryTime).GetInterval();

      auto& sRec = s.GetRecData();
      const double sigma2 =
        timeVariance.GetTimeSigma2(sRec.GetTotalSignal(), sRec.GetT50(), cosTheta, sqrt(RPerp2(axis, vx))) +
        sRec.GetGPSTimeVariance();
      const double residual = (t - t0 + vx*axis_c) / std::sqrt(sigma2);
      sRec.SetResidual(residual);
      sum += residual;
      chi2 += Sqr(residual);
      ++n;
    }

    mean = sum / n;
    spread = sqrt(chi2/(n - 1));
  }


  VModule::ResultFlag
  SdPlaneFit::PlaneFit3DDriver(FitParameters& fit)
    const
  {
    TMinuit minuit(3);
    int errFlag = 0;
    double argList[10];
    argList[0] = -1;
    if (!fMinuitOutput) {
      minuit.mnexcm("SET PRINTOUT", argList, 1, errFlag);
      minuit.mnexcm("SET NOWARNINGS", argList, 0, errFlag);
      minuit.SetPrintLevel(-1);
    }

    minuit.SetFCN(SdPlaneFit::PlaneFit3DFnc);
    argList[0] = 1; // chi2
    minuit.mnexcm("SET ERRORDEF", argList, 1, errFlag);

    minuit.mnparm(0, "u", fit.u, 0.01, 0, 0, errFlag);
    minuit.mnparm(1, "v", fit.v, 0.01, 0, 0, errFlag);
    minuit.mnparm(2, "ct0", fit.ct0, 10*meter, 0, 0, errFlag);

    // init fnc
    argList[0] = 1;
    minuit.mnexcm("CALI", argList, 1, errFlag);

    argList[0] = 500;
    minuit.mnexcm("MINIMIZE", argList, 1, errFlag);
    if (errFlag) {
      OUT(eObscure)
        << "  minuit minimize error: " << errFlag << '\n';
      minuit.mnexcm("MINOS", argList, 0, errFlag);
      if (errFlag) {
        OUT(eObscure)
          << "  minuit minos error: " << errFlag << '\n';
        return eFailure;
      }
    }

    // Get results
    double foo = 0;
    minuit.GetParameter(0, fit.u, foo);
    minuit.GetParameter(1, fit.v, foo);
    const double w2 = 1 - Sqr(fit.u) - Sqr(fit.v);
    if (w2 < 0) {
      OUT(eIntermediate)
        << "  Fit failed due to unphysical angle cosines." << endl;
      return eFailure;
    }
    fit.w = sqrt(w2);
    minuit.GetParameter(2, fit.ct0, foo);

    // error matrix
    double emat[3][3];
    minuit.mnemat(&emat[0][0], 3);
    fit.sigmaU2 = emat[0][0];
    fit.sigmaV2 = emat[1][1];
    fit.sigmaUV = emat[0][1];
    fit.sigmact02 = emat[2][2];

    fit.stage = ShowerSRecData::kPlaneFit3d;

    return eSuccess;
  }


  void
  SdPlaneFit::PlaneFit3DFnc(int& /*nPar*/, double* const /*grad*/, double& value,
                            double* const par, const int flag)
  {
    const double& u = par[0];
    const double& v = par[1];
    const double& ct0 = par[2];

    struct StationData {
      double fCTiming;
      double fT50;
      double fGPSVar;
      double fSignal;
      Vector fPosition;
    };

    static vector<StationData> sData;
    static const sdet::STimeVariance* timeVariance = nullptr;

    if (flag == 1) {  // fnc init
      const auto& sDetector = det::Detector::GetInstance().GetSDetector();
      sData.clear();
      for (const auto& s : fgCurrentSEvent->CandidateStationsRange()) {
        const auto& sRec = s.GetRecData();
        sData.emplace_back(StationData{
          kSpeedOfLight * (sRec.GetSignalStartTime() - fgBaryTime).GetInterval(),
          sRec.GetT50(),
          sRec.GetGPSTimeVariance(),
          sRec.GetTotalSignal(),
          sDetector.GetStation(s).GetPosition() - fgBarycenter
        });
      }
      timeVariance = &sdet::STimeVariance::GetInstance();
    }

    const double w2 = 1 - Sqr(u) - Sqr(v);

    if (w2 < 0) {
      WARNING("complex w");
      value = 1e30;
      return;
    }

    const double w = sqrt(w2);

    const Vector axis(u,v,w, fgBarycenterCS);

    double chi2 = 0;

    for (const auto& d : sData) {
      chi2 +=
        Sqr(d.fCTiming - ct0 + axis * d.fPosition) /
          (timeVariance->GetTimeSigma2(d.fSignal, d.fT50, w, std::sqrt(RPerp2(axis, d.fPosition))) + d.fGPSVar);
    }

    value = chi2 / kSpeedOfLight2;
  }

}