From 88dc48d24b2d97136ad73632332ce10d7f5a16de Mon Sep 17 00:00:00 2001
From: Nora <n.bluhme@gsi.de>
Date: Tue, 28 Nov 2023 14:59:57 +0000
Subject: [PATCH] Residual histograms in alignment

---
 reco/alignment/CbmBbaAlignmentTask.cxx | 143 ++++++++++++++++++-------
 reco/alignment/CbmBbaAlignmentTask.h   |  58 +++++-----
 2 files changed, 133 insertions(+), 68 deletions(-)

diff --git a/reco/alignment/CbmBbaAlignmentTask.cxx b/reco/alignment/CbmBbaAlignmentTask.cxx
index 3df2712526..90b3ef89d7 100644
--- a/reco/alignment/CbmBbaAlignmentTask.cxx
+++ b/reco/alignment/CbmBbaAlignmentTask.cxx
@@ -16,7 +16,6 @@
 // ROOT headers
 
 #include "FairRootManager.h"
-
 #include "TClonesArray.h"
 #include "TFile.h"
 #include "TH1F.h"
@@ -38,11 +37,11 @@
 #include "CbmTofTrackingInterface.h"
 #include "CbmTrdTrack.h"
 #include "CbmTrdTrackingInterface.h"
+#include "TCanvas.h"
+#include "bba/BBA.h"
 
 #include <cmath>
 
-#include "bba/BBA.h"
-
 // c++ and std headers
 
 #include <iostream>
@@ -79,7 +78,9 @@ CbmBbaAlignmentTask::CbmBbaAlignmentTask(const char* name, Int_t iVerbose, TStri
   TFile* curFile           = gFile;
   TDirectory* curDirectory = gDirectory;
 
-  if (!(fHistoFileName == "")) { fHistoFile = new TFile(fHistoFileName.Data(), "RECREATE"); }
+  if (!(fHistoFileName == "")) {
+    fHistoFile = new TFile(fHistoFileName.Data(), "RECREATE");
+  }
   else {
     fHistoFile = gFile;
   }
@@ -89,8 +90,6 @@ CbmBbaAlignmentTask::CbmBbaAlignmentTask(const char* name, Int_t iVerbose, TStri
   fHistoDir = fHistoFile->mkdir("Bba");
   fHistoDir->cd();
 
-  hTestHisto = new TH1F("hTestHisto", "hTestHisto", 100, 0., 1.);
-
   gFile      = curFile;
   gDirectory = curDirectory;
 }
@@ -152,6 +151,27 @@ InitStatus CbmBbaAlignmentTask::Init()
 
   fTracks.clear();
 
+
+  ca::Framework* l1             = CbmL1::Instance()->fpAlgo;
+  const ca::Parameters& l1Param = l1->GetParameters();
+  fNalignmentBodies             = l1Param.GetNstationsActive();
+
+  TDirectory* curDirectory = gDirectory;
+
+  fHistoDir->cd();
+  for (int i = 0; i < fNalignmentBodies; i++) {
+    hResidualsBeforeAlignmentX.push_back(
+      new TH1F(Form("BeforeAliBody%d_X", i), Form("X-Residuals Before Alignment, body %d", i), 100, -0.5, 0.5));
+    hResidualsBeforeAlignmentY.push_back(
+      new TH1F(Form("BeforeAliBody%d_Y", i), Form("Y-Residuals Before Alignment, body %d", i), 100, -0.5, 0.5));
+    hResidualsAfterAlignmentX.push_back(
+      new TH1F(Form("AfterAliBody%d_X", i), Form("X-Residuals After Alignment, body %d", i), 100, -0.5, 0.5));
+    hResidualsAfterAlignmentY.push_back(
+      new TH1F(Form("AfterAliBody%d_Y", i), Form("Y-Residuals After Alignment, body %d", i), 100, -0.5, 0.5));
+  }
+
+  gDirectory = curDirectory;
+
   return kSUCCESS;
 }
 
@@ -162,7 +182,9 @@ void CbmBbaAlignmentTask::Exec(Option_t* /*opt*/)
 
   fNEvents++;
 
-  if (static_cast<int>(fTracks.size()) >= fMaxNtracks) { return; }
+  if (static_cast<int>(fTracks.size()) >= fMaxNtracks) {
+    return;
+  }
 
   ca::Framework* l1           = CbmL1::Instance()->fpAlgo;
   const ca::Parameters& l1Par = l1->GetParameters();
@@ -173,7 +195,9 @@ void CbmBbaAlignmentTask::Exec(Option_t* /*opt*/)
   // select tracks for alignment and store them
   if (fTrackingMode == kSts && fInputStsTracks) {
     for (int iTr = 0; iTr < fInputStsTracks->GetEntriesFast(); iTr++) {
-      if (static_cast<int>(fTracks.size()) >= fMaxNtracks) { break; }
+      if (static_cast<int>(fTracks.size()) >= fMaxNtracks) {
+        break;
+      }
       const CbmStsTrack* stsTrack = dynamic_cast<CbmStsTrack*>(fInputStsTracks->At(iTr));
       if (!stsTrack) continue;
       TrackContainer t;
@@ -190,7 +214,9 @@ void CbmBbaAlignmentTask::Exec(Option_t* /*opt*/)
   }
   else if (fTrackingMode == kMcbm && fInputGlobalTracks) {
     for (int iTr = 0; iTr < fInputGlobalTracks->GetEntriesFast(); iTr++) {
-      if (static_cast<int>(fTracks.size()) >= fMaxNtracks) { break; }
+      if (static_cast<int>(fTracks.size()) >= fMaxNtracks) {
+        break;
+      }
       const CbmGlobalTrack* globalTrack = dynamic_cast<const CbmGlobalTrack*>(fInputGlobalTracks->At(iTr));
       if (!globalTrack) {
         LOG(fatal) << "BBA: null pointer to the global track!";
@@ -271,7 +297,9 @@ double CbmBbaAlignmentTask::CostFunction(const std::vector<double>& par)
   }
 
   double cost = fChi2Total / (fNdfTotal + 1);
-  if (fFixedNdf > 0 && fNdfTotal != fFixedNdf) { cost = 1.e30; }
+  if (fFixedNdf > 0 && fNdfTotal != fFixedNdf) {
+    cost = 1.e30;
+  }
   LOG(info) << "BBA: cost function:  ndf " << fNdfTotal << ", cost " << cost
             << ", diff to ideal cost: " << cost - fCostIdeal;
 
@@ -286,22 +314,16 @@ void CbmBbaAlignmentTask::Finish()
 
   LOG(info) << "BBA: start the alignment procedure with " << fTracks.size() << " tracks ...";
 
-  ca::Framework* l1 = CbmL1::Instance()->fpAlgo;
-
-  const ca::Parameters& l1Param = l1->GetParameters();
-
-  // init alignmemt module
-
-  int nStations = l1Param.GetNstationsActive();
-
   for (auto& t : fTracks) {
     for (auto& n : t.fUnalignedTrack.fNodes) {
-      n.fReference1 = n.fMaterialLayer;  // material layer is currently equal to the active tracking station
+      // TODO: get the body index from n.fHitSystemId, n.fHitAddress
+      int iBody     = n.fMaterialLayer;  // material layer is currently equal to the active tracking station
+      n.fReference1 = iBody;             // reference1 will be the body index
     }
     t.fAlignedTrack = t.fUnalignedTrack;
   }
 
-  int nParameters = 3 * nStations;  // x, y, z
+  int nParameters = 3 * fNalignmentBodies;  // x, y, z
 
   std::vector<bba::Parameter> par(nParameters);
 
@@ -309,30 +331,30 @@ void CbmBbaAlignmentTask::Finish()
     bba::Parameter& p = par[ip];
     p.SetActive(0);
     p.SetValue(0.);
-    p.SetBoundaryMin(-2.);  // +-1 cm alignment range
-    p.SetBoundaryMax(2.);
-    p.SetStepMin(1.e-6);
+    p.SetBoundaryMin(-4.);  // +-1 cm alignment range
+    p.SetBoundaryMax(4.);
+    p.SetStepMin(1.e-4);
     p.SetStepMax(0.1);
-    p.SetStep(100.e-4);
+    p.SetStep(1.e-2);
   }
 
   par[0].SetActive(0);  // fix the first station
   par[1].SetActive(0);
   par[2].SetActive(0);
 
-  par[3 * (nStations - 1) + 0].SetActive(0);  // fix the last station
-  par[3 * (nStations - 1) + 1].SetActive(0);
-  par[3 * (nStations - 1) + 2].SetActive(0);
+  par[3 * (fNalignmentBodies - 1) + 0].SetActive(0);  // fix the last station
+  par[3 * (fNalignmentBodies - 1) + 1].SetActive(0);
+  par[3 * (fNalignmentBodies - 1) + 2].SetActive(0);
 
   // set active parameters for other stations
 
-  for (int i = 1; i < nStations - 1; i++) {
+  for (int i = 1; i < fNalignmentBodies - 1; i++) {
     par[3 * i + 0].SetActive(0);
     par[3 * i + 1].SetActive(0);
     par[3 * i + 2].SetActive(0);
   }
 
-  for (int i = 1; i < nStations - 1; i++) {
+  for (int i = 1; i < fNalignmentBodies - 1; i++) {
     par[3 * i + 0].SetActive(1);
     par[3 * i + 1].SetActive(1);
     par[3 * i + 2].SetActive(1);
@@ -340,16 +362,22 @@ void CbmBbaAlignmentTask::Finish()
 
   gRandom->SetSeed(1);
 
-  for (int is = 0; is < nStations; is++) {
+  for (int is = 0; is < fNalignmentBodies; is++) {
     bba::Parameter& px = par[3 * is + 0];
     bba::Parameter& py = par[3 * is + 1];
     bba::Parameter& pz = par[3 * is + 2];
 
     // +- 0.5 cm misalignment
-    double d = 10.;
-    if (px.IsActive()) { px.SetValue(gRandom->Uniform(2. * d) - d); }
-    if (py.IsActive()) { py.SetValue(gRandom->Uniform(2. * d) - d); }
-    if (pz.IsActive()) { pz.SetValue(gRandom->Uniform(2. * d) - d); }
+    double d = 4.;
+    if (px.IsActive()) {
+      px.SetValue(gRandom->Uniform(2. * d) - d);
+    }
+    if (py.IsActive()) {
+      py.SetValue(gRandom->Uniform(2. * d) - d);
+    }
+    if (pz.IsActive()) {
+      pz.SetValue(gRandom->Uniform(2. * d) - d);
+    }
   }
 
   bba::BBA alignment;
@@ -361,31 +389,49 @@ void CbmBbaAlignmentTask::Finish()
   alignment.setChi2(lambdaCostFunction);
   alignment.printInfo();
 
-  std::vector<double> parAligned;
+  std::vector<double> parIdeal;
   std::vector<double> parMisaligned;
   {
     const std::vector<double>& r = alignment.getResult();
     for (unsigned int i = 0; i < r.size(); i++) {
       parMisaligned.push_back(r[i]);
-      parAligned.push_back(0.);
+      parIdeal.push_back(0.);
     }
   }
 
   {
     std::cout << "initial cost function..." << std::endl;
+
     fCostInitial = CostFunction(parMisaligned);
+
     for (auto& t : fTracks) {
       double ndf  = t.fAlignedTrack.fNodes[t.fAlignedTrack.fFirstHitNode].fTrack.GetNdf()[0];
       double chi2 = t.fAlignedTrack.fNodes[t.fAlignedTrack.fFirstHitNode].fTrack.GetChiSq()[0];
-      if (ndf < 0. || chi2 < 0. || !std::isfinite(ndf) || !std::isfinite(chi2)) { t.fIsActive = false; }
+      if (ndf < 0. || chi2 < 0. || !std::isfinite(ndf) || !std::isfinite(chi2)) {
+        t.fIsActive = false;
+      }
     }
     std::cout << "recalculate initial cost function..." << std::endl;
     fCostInitial = CostFunction(parMisaligned);
     fFixedNdf    = -1;  //fNdfTotal;
+
+
+    for (auto& t : fTracks) {
+      std::cout << "THIS IS THE HISTOGRAM LOOP" << std::endl;
+      if (!t.fIsActive) continue;
+      // calculate the residuals for all tracks at each fitNode before alignment
+      for (unsigned int in = 0; in < t.fAlignedTrack.fNodes.size(); in++) {
+        CbmKfTrackFitter::FitNode& node = t.fAlignedTrack.fNodes[in];
+        double x_residual = node.fMxy.X()[0] - node.fTrack.X()[0];  // this should be the difference vector
+        double y_residual = node.fMxy.Y()[0] - node.fTrack.Y()[0];
+        hResidualsBeforeAlignmentX[node.fReference1]->Fill(x_residual);
+        hResidualsBeforeAlignmentY[node.fReference1]->Fill(y_residual);
+      }
+    }
   }
 
   std::cout << "ideal cost function..." << std::endl;
-  fCostIdeal = CostFunction(parAligned);
+  fCostIdeal = CostFunction(parIdeal);
 
 
   LOG(info) << "Initial nTracks: " << fTracks.size() << " cost " << fCostInitial;
@@ -396,18 +442,32 @@ void CbmBbaAlignmentTask::Finish()
   LOG(info) << " cost function for the true parameters: " << fCostIdeal;
 
   LOG(info) << " Misaligned parameters: ";
-  for (int is = 0; is < nStations; is++) {
+  for (int is = 0; is < fNalignmentBodies; is++) {
     const std::vector<double>& r = parMisaligned;
     LOG(info) << "Sts station " << is << ": x " << r[3 * is + 0] << " y " << r[3 * is + 1] << " z " << r[3 * is + 2];
   }
 
   LOG(info) << " Alignment results: ";
 
-  for (int is = 0; is < nStations; is++) {
+  for (int is = 0; is < fNalignmentBodies; is++) {
     const std::vector<double>& r = alignment.getResult();
     LOG(info) << "Sts station " << is << ": x " << r[3 * is + 0] << " y " << r[3 * is + 1] << " z " << r[3 * is + 2];
   }
 
+  CostFunction(alignment.getResult());
+
+  for (auto& t : fTracks) {
+    if (!t.fIsActive) continue;
+    // calculate the residuals for all tracks at each fitNode before alignment
+    for (unsigned int in = 0; in < t.fAlignedTrack.fNodes.size(); in++) {
+      CbmKfTrackFitter::FitNode& node = t.fAlignedTrack.fNodes[in];
+      double x_residual               = node.fMxy.X()[0] - node.fTrack.X()[0];
+      double y_residual               = node.fMxy.Y()[0] - node.fTrack.Y()[0];
+      hResidualsAfterAlignmentX[node.fReference1]->Fill(x_residual);
+      hResidualsAfterAlignmentY[node.fReference1]->Fill(y_residual);
+    }
+  }
+
   // store the histograms
 
   TDirectory* curr   = gDirectory;
@@ -426,7 +486,8 @@ void CbmBbaAlignmentTask::Finish()
 
 void CbmBbaAlignmentTask::WriteHistosCurFile(TObject* obj)
 {
-  if (!obj->IsFolder()) obj->Write();
+  if (!obj->IsFolder())
+    obj->Write();
   else {
     TDirectory* cur    = gDirectory;
     TFile* currentFile = gFile;
diff --git a/reco/alignment/CbmBbaAlignmentTask.h b/reco/alignment/CbmBbaAlignmentTask.h
index dd03c17f61..c66cbc2487 100644
--- a/reco/alignment/CbmBbaAlignmentTask.h
+++ b/reco/alignment/CbmBbaAlignmentTask.h
@@ -14,9 +14,7 @@
 
 
 #include "CbmKfTrackFitter.h"
-
 #include "FairTask.h"
-
 #include "TString.h"
 
 #include <vector>
@@ -38,7 +36,7 @@ class TH1F;
 /// typedef Vc::double_v fvec;
 ///
 class CbmBbaAlignmentTask : public FairTask {
-public:
+ public:
   // Constructors/Destructors ---------
   CbmBbaAlignmentTask(const char* name = "CbmBbaAlignmentTask", Int_t iVerbose = 0,
                       TString histoFileName = "CbmBbaAlignmentHisto.root");
@@ -53,7 +51,7 @@ public:
   void SetMcbmTrackingMode() { fTrackingMode = TrackingMode::kMcbm; }
   void SetStsTrackingMode() { fTrackingMode = TrackingMode::kSts; }
 
-public:
+ public:
   enum TrackingMode
   {
     kSts,
@@ -67,17 +65,17 @@ public:
     CbmKfTrackFitter::Track fUnalignedTrack;  // track before alignment
     CbmKfTrackFitter::Track fAlignedTrack;    // track after alignment
 
-    int fNmvdHits {0};   // number of MVD hits
-    int fNstsHits {0};   // number of STS hits
-    int fNmuchHits {0};  // number of MUCH hits
-    int fNtrdHits {0};   // number of TRD hits
-    int fNtofHits {0};   // number of TOF hits
+    int fNmvdHits{0};   // number of MVD hits
+    int fNstsHits{0};   // number of STS hits
+    int fNmuchHits{0};  // number of MUCH hits
+    int fNtrdHits{0};   // number of TRD hits
+    int fNtofHits{0};   // number of TOF hits
 
-    bool fIsActive {1};  // is the track active
+    bool fIsActive{1};  // is the track active
     void MakeConsistent();
   };
 
-private:
+ private:
   const CbmBbaAlignmentTask& operator=(const CbmBbaAlignmentTask&);
   CbmBbaAlignmentTask(const CbmBbaAlignmentTask&);
 
@@ -91,12 +89,12 @@ private:
 
   // input data arrays
 
-  TClonesArray* fInputGlobalTracks {nullptr};
-  TClonesArray* fInputStsTracks {nullptr};
+  TClonesArray* fInputGlobalTracks{nullptr};
+  TClonesArray* fInputStsTracks{nullptr};
 
-  TClonesArray* fInputMcTracks {nullptr};            // Mc info for debugging
-  TClonesArray* fInputGlobalTrackMatches {nullptr};  // Mc info for debugging
-  TClonesArray* fInputStsTrackMatches {nullptr};     // Mc info for debugging
+  TClonesArray* fInputMcTracks{nullptr};            // Mc info for debugging
+  TClonesArray* fInputGlobalTrackMatches{nullptr};  // Mc info for debugging
+  TClonesArray* fInputStsTrackMatches{nullptr};     // Mc info for debugging
 
   CbmKfTrackFitter fFitter;
 
@@ -104,23 +102,29 @@ private:
   std::vector<TrackContainer> fTracks;
 
   //output file with histograms
-  TString fHistoFileName {"CbmBbaAlignmentHisto.root"};
-  TFile* fHistoFile {nullptr};
-  TDirectory* fHistoDir {nullptr};
+  TString fHistoFileName{"CbmBbaAlignmentHisto.root"};
+  TFile* fHistoFile{nullptr};
+  TDirectory* fHistoDir{nullptr};
 
-  Int_t fNEvents {0};
+  Int_t fNEvents{0};
 
-  Int_t fMaxNtracks {10000};
+  Int_t fMaxNtracks{10000};
 
-  double fCostIdeal {1.e10};
-  double fCostInitial {0.};
+  int fNalignmentBodies{0};
 
-  double fChi2Total {0.};
-  long fNdfTotal {0};
-  long fFixedNdf {-1};
+  double fCostIdeal{1.e10};
+  double fCostInitial{0.};
+
+  double fChi2Total{0.};
+  long fNdfTotal{0};
+  long fFixedNdf{-1};
 
   //histograms
-  TH1F* hTestHisto {nullptr};
+
+  std::vector<TH1F*> hResidualsBeforeAlignmentX{};
+  std::vector<TH1F*> hResidualsBeforeAlignmentY{};
+  std::vector<TH1F*> hResidualsAfterAlignmentX{};
+  std::vector<TH1F*> hResidualsAfterAlignmentY{};
 
   ClassDef(CbmBbaAlignmentTask, 1);
 };
-- 
GitLab