FMD/AliFMDSurveyToAlignObjs.cxx

   1 #include "AliFMDSurveyToAlignObjs.h"
   2 #include "AliLog.h"
   3 #include "AliSurveyPoint.h"
   4 #include <TGraph2DErrors.h>
   5 #include <TF2.h>
   6 #include <TVector3.h>
   7 #include <iostream>
   8 #include <iomanip>
   9 #include <TMath.h>
  10 #include <TRotation.h>
  11 #include <TGeoMatrix.h>
  12 #include <TGeoManager.h>
  13 #include <TGeoPhysicalNode.h>
  14 #include "AliFMDGeometry.h"
  15
  16 //____________________________________________________________________
  17 Double_t
  18 AliFMDSurveyToAlignObjs::GetUnitFactor() const
  19 {
  20   // Returns the conversion factor from the measured values to
  21   // centimeters.
  22   if (!fSurveyObj) return 0;
  23   TString units(fSurveyObj->GetUnits());
  24   if      (units.CompareTo("mm", TString::kIgnoreCase) == 0) return .1;
  25   else if (units.CompareTo("cm", TString::kIgnoreCase) == 0) return 1.;
  26   else if (units.CompareTo("m",  TString::kIgnoreCase) == 0) return 100.;
  27   return 1;
  28 }
  29
  30 //____________________________________________________________________
  31 Bool_t
  32 AliFMDSurveyToAlignObjs::GetPoint(const char* name,
  33                                   TVector3&   point,
  34                                   TVector3&   error) const
  35 {
  36   // Get named point.   On return, point will contain the point
  37   // coordinates in centimeters, and error will contain the
  38   // meassurement errors in centimeters too.  If no point is found,
  39   // returns false, otherwise true.
  40   if (!fSurveyPoints) return kFALSE;
  41
  42   Double_t unit = GetUnitFactor();
  43   if (unit == 0) return kFALSE;
  44
  45   TObject* obj  = fSurveyPoints->FindObject(name);
  46   if (!obj) return kFALSE;
  47
  48   AliSurveyPoint* p = static_cast<AliSurveyPoint*>(obj);
  49   point.SetXYZ(unit * p->GetX(),
  50                unit * p->GetY(),
  51                unit * p->GetZ());
  52   error.SetXYZ(unit * p->GetPrecisionX(),
  53                unit * p->GetPrecisionY(),
  54                unit * p->GetPrecisionZ());
  55   return kTRUE;
  56 }
  57
  58 //____________________________________________________________________
  59 Bool_t
  60 AliFMDSurveyToAlignObjs::CalculatePlane(const     TVector3& a,
  61                                         const     TVector3& b,
  62                                         const     TVector3& c,
  63                                         Double_t  depth,
  64                                         Double_t* trans,
  65                                         Double_t* rot) const
  66 {
  67   // Vector a->b, b->c, and normal to plane defined by these two
  68   // vectors.
  69   TVector3 ab(b-a), bc(c-b);
  70
  71   // Normal vector to the plane of the fiducial marks obtained
  72   // as cross product of the two vectors on the plane d0^d1
  73   TVector3 nn(ab.Cross(bc));
  74   if (nn.Mag() < 1e-8) {
  75     Info("CalculatePlane", "Normal vector is null vector");
  76     return kFALSE;
  77   }
  78
  79   // We express the plane in Hessian normal form.
  80   //
  81   //   n x = -p,
  82   //
  83   // where n is the normalised normal vector given by
  84   //
  85   //   n_x = a / l,   n_y = b / l,   n_z = c / l,  p = d / l
  86   //
  87   // with l = sqrt(a^2+b^2+c^2) and a, b, c, and d are from the
  88   // normal plane equation
  89   //
  90   //  ax + by + cz + d = 0
  91   //
  92   // Normalize
  93   TVector3 n(nn.Unit());
  94   // Double_t p = - (n * a);
  95
  96   // The center of the square with the fiducial marks as the
  97   // corners.  The mid-point of one diagonal - md.  Used to get the
  98   // center of the surveyd box.
  99   TVector3 md(a + c);
 100   md *= 1/2.;
 101
 102   // The center of the box.
 103   TVector3 orig(md - depth * n);
 104   trans[0] = orig[0];
 105   trans[1] = orig[1];
 106   trans[2] = orig[2];
 107
 108   // Normalize the spanning vectors
 109   TVector3 uab(ab.Unit());
 110   TVector3 ubc(bc.Unit());
 111
 112   for (size_t i = 0; i < 3; i++) {
 113     rot[i * 3 + 0] = ubc[i];
 114     rot[i * 3 + 1] = uab[i];
 115     rot[i * 3 + 2] = n[i];
 116   }
 117   return kTRUE;
 118 }
 119
 120 //____________________________________________________________________
 121 Bool_t
 122 AliFMDSurveyToAlignObjs::FitPlane(const TObjArray& points,
 123                                   const TObjArray& errors,
 124                                   Double_t         /* depth */,
 125                                   Double_t*        trans,
 126                                   Double_t*        rot) const
 127 {
 128   Int_t nPoints = points.GetEntries();
 129   if (nPoints < 4) {
 130     AliError(Form("Cannot fit a plane equation to less than 4 survey points, "
 131                   "got only %d", nPoints));
 132     return kFALSE;
 133   }
 134
 135   TGraph2DErrors g;
 136   // Loop and fill graph
 137   for (int i = 0; i < nPoints; i++) {
 138     TVector3* p = static_cast<TVector3*>(points.At(i));
 139     TVector3* e = static_cast<TVector3*>(errors.At(i));
 140
 141     if (!p || !e) continue;
 142
 143     g.SetPoint(i, p->X(), p->Y(), p->Z());
 144     g.SetPointError(i, e->X(), e->Y(), e->Z());
 145   }
 146
 147   // Check that we have enough points
 148   if (g.GetN() < 4) {
 149     AliError(Form("Only got %d survey points - no good for plane fit",
 150                   g.GetN()));
 151     return kFALSE;
 152   }
 153
 154   // Next, declare fitting function and fit to graph.
 155   // Fit to the plane equation:
 156   //
 157   //   ax + by + cz + d = 0
 158   //
 159   // or
 160   //
 161   //   z = - ax/c - by/c - d/c
 162   //
 163   TF2 f("plane", "-[0]*x-[1]*y-[2]",
 164         g.GetXmin(), g.GetXmax(), g.GetYmin(), g.GetYmax());
 165   g.Fit(&f, "Q");
 166
 167   // Now, extract the normal and offset
 168   TVector3 nv(f.GetParameter(0), f.GetParameter(1), 1);
 169   TVector3 n(nv.Unit());
 170   Double_t p = -f.GetParameter(2);
 171
 172   // Create two vectors spanning the plane
 173   TVector3 a(1, 0, f.Eval(1, 0)-p);
 174   TVector3 b(0, -1, f.Eval(0, -1)-p);
 175   TVector3 ua(a.Unit());
 176   TVector3 ub(b.Unit());
 177   // Double_t angAb = ua.Angle(ub);
 178   // PrintVector("ua: ", ua);
 179   // PrintVector("ub: ", ub);
 180   // std::cout << "Angle: " << angAb * 180 / TMath::Pi() << std::endl;
 181
 182   for (size_t i = 0; i < 3; i++) {
 183     rot[i * 3 + 0] = ua[i];
 184     rot[i * 3 + 1] = ub[i];
 185     rot[i * 3 + 2] = n[i];
 186   }
 187
 188   // The intersection of the plane is given by (0, 0, -d/c)
 189   trans[0] = 0;
 190   trans[1] = 0;
 191   trans[2] = p;
 192
 193   return kTRUE;
 194 }
 195
 196
 197 //____________________________________________________________________
 198 Bool_t
 199 AliFMDSurveyToAlignObjs::MakeDelta(const char*  path,
 200                                    Double_t*    rot,
 201                                    Double_t*    trans,
 202                                    TGeoHMatrix& delta) const
 203 {
 204   // Make delta transformation
 205   if (!gGeoManager)           return kFALSE;
 206   if (!gGeoManager->cd(path)) return kFALSE;
 207
 208
 209   TGeoMatrix*  global = gGeoManager->GetCurrentMatrix();
 210 #if 0
 211   PrintRotation(Form("%s rot:", global->GetName()),global->GetRotationMatrix());
 212   PrintVector(Form("%s trans:", global->GetName()),global->GetTranslation());
 213 #endif
 214
 215   return MakeDelta(global, rot, trans, delta);
 216 }
 217
 218 //____________________________________________________________________
 219 Bool_t
 220 AliFMDSurveyToAlignObjs::MakeDelta(TGeoMatrix*  global,
 221                                    Double_t*    rot,
 222                                    Double_t*    trans,
 223                                    TGeoHMatrix& delta) const
 224 {
 225   // Make delta transformation
 226   TGeoHMatrix* geoM = new TGeoHMatrix;
 227   geoM->SetTranslation(trans);
 228   geoM->SetRotation(rot);
 229
 230   delta = global->Inverse();
 231   delta.MultiplyLeft(geoM);
 232
 233   return true;
 234 }
 235
 236 //____________________________________________________________________
 237 Bool_t
 238 AliFMDSurveyToAlignObjs::GetFMD1Plane(Double_t* rot, Double_t* trans) const
 239 {
 240
 241   // The possile survey points
 242   TVector3  icb, ict, ocb, oct, dummy;
 243   Int_t     missing = 0;
 244   if (!GetPoint("V0L_ICB", icb, dummy)) missing++;
 245   if (!GetPoint("V0L_ICT", ict, dummy)) missing++;
 246   if (!GetPoint("V0L_OCB", ocb, dummy)) missing++;
 247   if (!GetPoint("V0L_OCT", oct, dummy)) missing++;
 248
 249   // Check that we have enough points
 250   if (missing > 1) {
 251     AliWarning(Form("Only got %d survey points - no good for FMD1 plane",
 252                     4-missing));
 253     return kFALSE;
 254   }
 255
 256 #if 0
 257   const char* lidN = "FMD1_lid_mat0";
 258   TGeoMatrix* lidM = static_cast<TGeoMatrix*>(gGeoManager->GetListOfMatrices()
 259                                               ->FindObject(lidN));
 260   if (!lidM) {
 261     AliError(Form("Couldn't find FMD1 lid transformation %s", lidN));
 262     return kFALSE;
 263   }
 264
 265   const Double_t* lidT = lidM->GetTranslation();
 266   Double_t        lidZ = lidT[2];
 267   Double_t        off  = lidZ-3.3;
 268 #else
 269   Double_t        off  = 0;
 270 #endif
 271
 272   if (!CalculatePlane(ocb, icb, ict, off, trans, rot)) return kFALSE;
 273   // PrintRotation("FMD1 rotation:",  rot);
 274   // PrintVector("FMD1 translation:", trans);
 275
 276   return kTRUE;
 277 }
 278
 279 //____________________________________________________________________
 280 Bool_t
 281 AliFMDSurveyToAlignObjs::DoFMD1()
 282 {
 283   // Do the FMD1 stuff
 284   Double_t rot[9], trans[3];
 285   if (!GetFMD1Plane(rot, trans)) return kFALSE;
 286   // const char* path = "/ALIC_1/F1MT_1/FMD1_lid_0";
 287
 288   // TGeoHMatrix delta;
 289   TGeoTranslation global(0,0,324.670);
 290   if (!MakeDelta(&global, rot, trans, fFMD1Delta))
 291     return kFALSE;
 292
 293   // PrintRotation("FMD1 delta rotation:",  fFMD1Delta.GetRotationMatrix());
 294   // PrintVector("FMD1 delta translation:", fFMD1Delta.GetTranslation());
 295
 296   return kTRUE;
 297 }
 298
 299 //____________________________________________________________________
 300 Bool_t
 301 AliFMDSurveyToAlignObjs::GetFMD2Plane(Double_t* rot, Double_t* trans) const
 302 {
 303
 304   // The possible survey points
 305   const char*    names[] = { "FMD2_ITOP",  "FMD2_OTOP",
 306                              "FMD2_IBOTM", "FMD2_OBOTM",
 307                              "FMD2_IBOT",  "FMD2_OBOT",
 308                              0 };
 309   const char**   name    = names;
 310
 311   TObjArray points;
 312   TObjArray errors;
 313
 314   // Loop and fill graph
 315   while (*name) {
 316     TVector3 p, e;
 317     if (!GetPoint(*name++, p, e)) continue;
 318
 319     points.Add(new TVector3(p));
 320     errors.Add(new TVector3(e));
 321   }
 322   if (points.GetEntries() < 4) {
 323     AliWarning(Form("Only got %d survey points - no good for FMD2 plane",
 324                     points.GetEntries()));
 325     return kFALSE;
 326   }
 327
 328   return FitPlane(points, errors, 0, trans, rot);
 329 }
 330
 331 #define M(I,J) rot[(J-1) * 3 + (I-1)]
 332 //____________________________________________________________________
 333 Bool_t
 334 AliFMDSurveyToAlignObjs::DoFMD2()
 335 {
 336   // Do the FMD2 stuff
 337   Double_t rot[9], trans[3];
 338   if (!GetFMD2Plane(rot, trans)) return kFALSE;
 339   // PrintRotation("FMD2 rotation:",  rot);
 340   // PrintVector("FMD2 translation:", trans);
 341
 342   for (int i = 0; i < 3; i++) {
 343     for (int j = 0; j < 3; j++) {
 344       rot[i*3+j] = (i == j ? 1 : 0);
 345     }
 346   }
 347   trans[0] = trans[1] = 0;
 348   trans[2] += 0.015;
 349   // PrintRotation("FMD2 rotation:",  rot);
 350   // PrintVector("FMD2 translation:", trans);
 351
 352   // TGeoHMatrix delta;
 353   if (!MakeDelta("/ALIC_1/F2MT_2/FMD2_support_0/FMD2_back_cover_2",
 354                  rot, trans, fFMD2Delta)) return kFALSE;
 355
 356   // PrintRotation("FMD2 delta rotation:",  fFMD2Delta.GetRotationMatrix());
 357   // PrintVector("FMD2 delta translation:", fFMD2Delta.GetTranslation());
 358
 359   return kTRUE;
 360 }
 361
 362 //____________________________________________________________________
 363 void
 364 AliFMDSurveyToAlignObjs::Run()
 365 {
 366   AliFMDGeometry* geom = AliFMDGeometry::Instance();
 367   geom->Init();
 368   geom->InitTransformations();
 369
 370   DoFMD1();
 371   DoFMD2();
 372 }
 373
 374 //____________________________________________________________________
 375 Bool_t
 376 AliFMDSurveyToAlignObjs::CreateAlignObjs()
 377 {
 378   TClonesArray& array = *fAlignObjArray;
 379   Int_t         n     = array.GetEntriesFast();
 380
 381   if (!fFMD1Delta.IsIdentity()) {
 382     new (array[n++]) AliAlignObjParams("FMD1/FMD1_T", 0, fFMD1Delta, kTRUE);
 383     new (array[n++]) AliAlignObjParams("FMD1/FMD1_B", 0, fFMD1Delta, kTRUE);
 384   }
 385   if (!fFMD2Delta.IsIdentity()) {
 386     new (array[n++]) AliAlignObjParams("FMD2/FMD2_T", 0, fFMD2Delta, kTRUE);
 387     new (array[n++]) AliAlignObjParams("FMD2/FMD2_B", 0, fFMD2Delta, kTRUE);
 388   }
 389   // array.Print();
 390
 391   return kTRUE;
 392 }
 393
 394 //____________________________________________________________________
 395 void
 396 AliFMDSurveyToAlignObjs::PrintVector(const char* text, const TVector3& v)
 397 {
 398   // Print a vector
 399   Double_t va[] = { v.X(), v.Y(), v.Z() };
 400   PrintVector(text, va);
 401 }
 402 //____________________________________________________________________
 403 void
 404 AliFMDSurveyToAlignObjs::PrintVector(const char* text, const Double_t* v)
 405 {
 406   // Print a vector
 407   std::cout << text
 408             << std::setw(15) << v[0]
 409             << std::setw(15) << v[1]
 410             << std::setw(15) << v[2]
 411             << std::endl;
 412 }
 413
 414
 415 //____________________________________________________________________
 416 void
 417 AliFMDSurveyToAlignObjs::PrintRotation(const char* text, const Double_t* rot)
 418 {
 419   // Print a rotation matrix
 420   std::cout << text << std::endl;
 421   for (size_t i = 0; i < 3; i++) {
 422     for (size_t j = 0; j < 3; j++)
 423       std::cout << std::setw(15) << rot[i * 3 + j];
 424     std::cout << std::endl;
 425   }
 426 }
 427
 428 //____________________________________________________________________
 429 //
 430 // EOF
 431 //