FMD/AliFMDSurveyToAlignObjs.cxx

   1 #include "AliFMDSurveyToAlignObjs.h"
   2 #include "AliSurveyPoint.h"
   3 #include <TGraph2DErrors.h>
   4 #include <TF2.h>
   5 #include <TVector3.h>
   6 #include <iostream>
   7 #include <iomanip>
   8 #include <TMath.h>
   9 #include <TRotation.h>
  10 #include <TGeoMatrix.h>
  11 #include <TGeoManager.h>
  12 #include <TGeoPhysicalNode.h>
  13 #include "AliFMDGeometry.h"
  14
  15 //____________________________________________________________________
  16 Double_t
  17 AliFMDSurveyToAlignObjs::GetUnitFactor() const
  18 {
  19   // Returns the conversion factor from the measured values to
  20   // centimeters.
  21   TString units(fSurveyObj->GetUnits());
  22   if      (units.CompareTo("mm", TString::kIgnoreCase) == 0) return .1;
  23   else if (units.CompareTo("cm", TString::kIgnoreCase) == 0) return 1.;
  24   else if (units.CompareTo("m",  TString::kIgnoreCase) == 0) return 100.;
  25   return 1;
  26 }
  27
  28 //____________________________________________________________________
  29 Bool_t
  30 AliFMDSurveyToAlignObjs::GetPoint(const char* name,
  31                                   TVector3&   point,
  32                                   TVector3&   error) const
  33 {
  34   // Get named point.   On return, point will contain the point
  35   // coordinates in centimeters, and error will contain the
  36   // meassurement errors in centimeters too.  If no point is found,
  37   // returns false, otherwise true.
  38   Double_t unit = GetUnitFactor();
  39   TObject* obj  = fSurveyPoints->FindObject(name);
  40   if (!obj) return kFALSE;
  41
  42    AliSurveyPoint* p = static_cast<AliSurveyPoint*>(obj);
  43    point.SetXYZ(unit * p->GetX(),
  44                 unit * p->GetY(),
  45                 unit * p->GetZ());
  46    error.SetXYZ(unit * p->GetPrecisionX(),
  47                 unit * p->GetPrecisionY(),
  48                 unit * p->GetPrecisionZ());
  49    return kTRUE;
  50 }
  51
  52 //____________________________________________________________________
  53 Bool_t
  54 AliFMDSurveyToAlignObjs::CalculatePlane(const     TVector3& a,
  55                                         const     TVector3& b,
  56                                         const     TVector3& c,
  57                                         Double_t* trans,
  58                                         Double_t* rot) const
  59 {
  60   // Vector a->b, b->c, and normal to plane defined by these two
  61   // vectors.
  62   TVector3 ab(b-a), bc(c-b);
  63
  64   // Normal vector to the plane of the fiducial marks obtained
  65   // as cross product of the two vectors on the plane d0^d1
  66   TVector3 nn(ab.Cross(bc));
  67   if (nn.Mag() < 1e-8) {
  68     Info("DoIt", "Normal vector is null vector");
  69     return kFALSE;
  70   }
  71
  72   // We express the plane in Hessian normal form.
  73   //
  74   //   n x = -p,
  75   //
  76   // where n is the normalised normal vector given by
  77   //
  78   //   n_x = a / l,   n_y = b / l,   n_z = c / l,  p = d / l
  79   //
  80   // with l = sqrt(a^2+b^2+c^2) and a, b, c, and d are from the
  81   // normal plane equation
  82   //
  83   //  ax + by + cz + d = 0
  84   //
  85   // Normalize
  86   TVector3 n(nn.Unit());
  87   Double_t p = - (n * a);
  88
  89   // The center of the square with the fiducial marks as the
  90   // corners.  The mid-point of one diagonal - md.  Used to get the
  91   // center of the surveyd box.
  92   TVector3 md(a + c);
  93   md *= 1/2.;
  94
  95   // The center of the box.
  96   TVector3 orig(md);
  97   trans[0] = orig[0];
  98   trans[1] = orig[1];
  99   trans[2] = orig[2];
 100
 101   // Normalize the spanning vectors
 102   TVector3 uab(ab.Unit());
 103   TVector3 ubc(bc.Unit());
 104
 105   for (size_t i = 0; i < 3; i++) {
 106     rot[i * 3 + 0] = uab[i];
 107     rot[i * 3 + 1] = ubc[i];
 108     rot[i * 3 + 2] = n[i];
 109   }
 110   return kTRUE;
 111 }
 112
 113
 114 //____________________________________________________________________
 115 Bool_t
 116 AliFMDSurveyToAlignObjs::GetFMD1Plane(Double_t* rot, Double_t* trans) const
 117 {
 118
 119   // The possile survey points
 120   TVector3  icb, ict, ocb, oct, dummy;
 121   Int_t     missing = 0;
 122   if (!GetPoint("V0L_ICB", icb, dummy)) missing++;
 123   if (!GetPoint("V0L_ICT", icb, dummy)) missing++;
 124   if (!GetPoint("V0L_OCB", icb, dummy)) missing++;
 125   if (!GetPoint("V0L_OCT", icb, dummy)) missing++;
 126
 127   // Check that we have enough points
 128   if (missing > 1) {
 129     Error("GetFMD1Plane", "Only got %d survey points - no good for FMD1 plane",
 130           4-missing);
 131     return kFALSE;
 132   }
 133
 134   if (!CalculatePlane(icb, ict, oct, rot, trans)) return kFALSE;
 135
 136   return kTRUE;
 137 }
 138
 139 //____________________________________________________________________
 140 Bool_t
 141 AliFMDSurveyToAlignObjs::DoFMD1()
 142 {
 143   return kTRUE;
 144 }
 145
 146 //____________________________________________________________________
 147 Bool_t
 148 AliFMDSurveyToAlignObjs::GetFMD2Plane(Double_t* rot, Double_t* trans) const
 149 {
 150
 151   // The possile survey points
 152   const char*    names[] = { "FMD2_ITOP",  "FMD2_OTOP",
 153                              "FMD2_IBOTM", "FMD2_OBOTM",
 154                              "FMD2_IBOT",  "FMD2_OBOT",
 155                              0 };
 156   const char**   name    = names;
 157   Int_t          i       = 0;
 158   TGraph2DErrors g;
 159   // Loop and fill graph
 160   while (*name) {
 161     TVector3 p, e;
 162     if (!GetPoint(*name++, p, e)) continue;
 163
 164     g.SetPoint(i, p.X(), p.Y(), p.Z());
 165     g.SetPointError(i, e.X(), e.Y(), e.Z());
 166     i++;
 167   }
 168
 169   // Check that we have enough points
 170   if (g.GetN() < 4) {
 171     Error("GetFMD2Plane", "Only got %d survey points - no good for FMD2 plane",
 172           g.GetN());
 173     return kFALSE;
 174   }
 175
 176   // Next, declare fitting function and fit to graph.
 177   // Fit to the plane equation:
 178   //
 179   //   ax + by + cz + d = 0
 180   //
 181   // or
 182   //
 183   //   z = - ax/c - by/c - d/c
 184   //
 185   TF2 f("fmd2Plane", "-[0]*x-[1]*y-[2]",
 186         g.GetXmin(), g.GetXmax(), g.GetYmin(), g.GetYmax());
 187   g.Fit(&f, "Q");
 188
 189   // Now, extract the normal and offset
 190   TVector3 nv(f.GetParameter(0), f.GetParameter(1), 1);
 191   TVector3 n(nv.Unit());
 192   Double_t p = -f.GetParameter(2);
 193
 194   // Create two vectors spanning the plane
 195   TVector3 a(1, 0, f.Eval(1, 0)-p);
 196   TVector3 b(0, -1, f.Eval(0, -1)-p);
 197   TVector3 ua(a.Unit());
 198   TVector3 ub(b.Unit());
 199   Double_t angAb = ua.Angle(ub);
 200   // PrintVector("ua: ", ua);
 201   // PrintVector("ub: ", ub);
 202   // std::cout << "Angle: " << angAb * 180 / TMath::Pi() << std::endl;
 203
 204   for (size_t i = 0; i < 3; i++) {
 205     rot[i * 3 + 0] = ua[i];
 206     rot[i * 3 + 1] = ub[i];
 207     rot[i * 3 + 2] = n[i];
 208   }
 209
 210   // The intersection of the plane is given by (0, 0, -d/c)
 211   trans[0] = 0;
 212   trans[1] = 0;
 213   trans[2] = p;
 214
 215   return kTRUE;
 216 }
 217
 218 #define M(I,J) rot[(J-1) * 3 + (I-1)]
 219 //____________________________________________________________________
 220 Bool_t
 221 AliFMDSurveyToAlignObjs::DoFMD2()
 222 {
 223   Double_t rot[9], trans[3];
 224   if (!GetFMD2Plane(rot, trans)) return kFALSE;
 225
 226   PrintRotation("Rotation: ", rot);
 227   PrintVector("Translation: ", trans);
 228
 229 #if 0
 230   Double_t theta = TMath::ATan2(M(3,1), M(3,2));
 231   Double_t phi   = TMath::ACos(M(3,3));
 232   Double_t psi   = -TMath::ATan2(M(1,3), M(2,3));
 233
 234   std::cout << " Theta=" << theta * 180 / TMath::Pi()
 235             << " Phi="   << phi   * 180 / TMath::Pi()
 236             << " Psi="   << psi   * 180 / TMath::Pi()
 237             << std::endl;
 238
 239   TRotation r;
 240   r.SetXEulerAngles(theta, phi, psi);
 241   r.Print();
 242
 243   TGeoRotation*   geoR = new TGeoRotation("FMD2_survey_rotation",
 244                                           r.GetYTheta(),
 245                                           r.GetYPhi(),
 246                                           r.GetYPsi());
 247   TGeoCombiTrans* geoM = new TGeoCombiTrans(trans[0], trans[1], trans[2], geoR);
 248 #else
 249   TGeoHMatrix* geoM = new TGeoHMatrix;
 250   geoM->SetRotation(rot);
 251   geoM->SetTranslation(trans);
 252 #endif
 253   geoM->Print();
 254
 255   const char* path = "/ALIC_1/F2MT_2/FMD2_support_0/FMD2_back_cover_2";
 256   gGeoManager->cd(path);
 257   TGeoMatrix* globalBack = gGeoManager->GetCurrentMatrix();
 258   globalBack->Print();
 259   PrintRotation("Back rotation:",    globalBack->GetRotationMatrix());
 260   PrintVector("Back translation:",   globalBack->GetTranslation());
 261
 262   // TObjArray* pns = gGeoManager->GetListOfPhysicalNodes();
 263   // TObject*   oT  = pns->FindObject("/ALIC_1/F2MT_2");
 264   // TObject*   oB  = pns->FindObject("/ALIC_1/F2MB_2");
 265   // if (!oT) {
 266   //   Warning("DoFMD2", Form("Physical node /ALIC_1/F2MT_2 not found"));
 267   //   return kFALSE;
 268   // }
 269   // if (!oB) {
 270   //   Warning("DoFMD2", Form("Physical node /ALIC_1/F2MB_2 not found"));
 271   //   return kFALSE;
 272   // }
 273   // TGeoPhysicalNode* top = static_cast<TGeoPhysicalNode*>(oT);
 274   // TGeoPhysicalNode* bot = static_cast<TGeoPhysicalNode*>(oB);
 275   TGeoHMatrix tDelta(globalBack->Inverse());
 276   TGeoHMatrix bDelta(globalBack->Inverse());
 277   PrintRotation("Back^-1 rotation:",    tDelta.GetRotationMatrix());
 278   PrintVector("Back^-1 translation:",   tDelta.GetTranslation());
 279
 280   std::cout << "tDelta = 1? " << tDelta.IsIdentity() << std::endl;
 281
 282   tDelta.MultiplyLeft(geoM);
 283   bDelta.MultiplyLeft(geoM);
 284
 285   PrintRotation("tDelta rotation:",  tDelta.GetRotationMatrix());
 286   PrintVector("tDelta translation:", tDelta.GetTranslation());
 287   PrintRotation("bDelta rotation:",  bDelta.GetRotationMatrix());
 288   PrintVector("bDelta translation:", bDelta.GetTranslation());
 289
 290   return kTRUE;
 291 }
 292
 293 //____________________________________________________________________
 294 void
 295 AliFMDSurveyToAlignObjs::Run()
 296 {
 297   AliFMDGeometry* geom = AliFMDGeometry::Instance();
 298   geom->Init();
 299   geom->InitTransformations();
 300
 301   DoFMD2();
 302 }
 303
 304 //____________________________________________________________________
 305 void
 306 AliFMDSurveyToAlignObjs::PrintVector(const char* text, const TVector3& v)
 307 {
 308   Double_t va[] = { v.X(), v.Y(), v.Z() };
 309   PrintVector(text, va);
 310 }
 311 //____________________________________________________________________
 312 void
 313 AliFMDSurveyToAlignObjs::PrintVector(const char* text, const Double_t* v)
 314 {
 315   std::cout << text
 316             << std::setw(15) << v[0]
 317             << std::setw(15) << v[1]
 318             << std::setw(15) << v[2]
 319             << std::endl;
 320 }
 321
 322
 323 //____________________________________________________________________
 324 void
 325 AliFMDSurveyToAlignObjs::PrintRotation(const char* text, const Double_t* rot)
 326 {
 327   std::cout << text << std::endl;
 328   for (size_t i = 0; i < 3; i++) {
 329     for (size_t j = 0; j < 3; j++)
 330       std::cout << std::setw(15) << rot[i * 3 + j];
 331     std::cout << std::endl;
 332   }
 333 }
 334
 335 //____________________________________________________________________
 336 //
 337 // EOF
 338 //