]>
Commit | Line | Data |
---|---|---|
b2e6f0b0 | 1 | #include "AliFMDSurveyToAlignObjs.h" |
f567c3ce | 2 | #include "AliLog.h" |
b2e6f0b0 | 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 | ||
faf80567 | 16 | //____________________________________________________________________ |
17 | Double_t | |
18 | AliFMDSurveyToAlignObjs::GetUnitFactor() const | |
19 | { | |
20 | // Returns the conversion factor from the measured values to | |
21 | // centimeters. | |
f567c3ce | 22 | if (!fSurveyObj) return 0; |
faf80567 | 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 | ||
b2e6f0b0 | 30 | //____________________________________________________________________ |
31 | Bool_t | |
faf80567 | 32 | AliFMDSurveyToAlignObjs::GetPoint(const char* name, |
33 | TVector3& point, | |
34 | TVector3& error) const | |
b2e6f0b0 | 35 | { |
faf80567 | 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. | |
f567c3ce | 40 | if (!fSurveyPoints) return kFALSE; |
41 | ||
faf80567 | 42 | Double_t unit = GetUnitFactor(); |
f567c3ce | 43 | if (unit == 0) return kFALSE; |
44 | ||
faf80567 | 45 | TObject* obj = fSurveyPoints->FindObject(name); |
46 | if (!obj) return kFALSE; | |
47 | ||
f567c3ce | 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; | |
faf80567 | 56 | } |
b2e6f0b0 | 57 | |
faf80567 | 58 | //____________________________________________________________________ |
59 | Bool_t | |
60 | AliFMDSurveyToAlignObjs::CalculatePlane(const TVector3& a, | |
61 | const TVector3& b, | |
62 | const TVector3& c, | |
f567c3ce | 63 | Double_t depth, |
faf80567 | 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); | |
b2e6f0b0 | 70 | |
faf80567 | 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) { | |
f567c3ce | 75 | Info("CalculatePlane", "Normal vector is null vector"); |
faf80567 | 76 | return kFALSE; |
77 | } | |
b2e6f0b0 | 78 | |
faf80567 | 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()); | |
f567c3ce | 94 | // Double_t p = - (n * a); |
faf80567 | 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.; | |
f567c3ce | 101 | |
faf80567 | 102 | // The center of the box. |
f567c3ce | 103 | TVector3 orig(md - depth * n); |
faf80567 | 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++) { | |
f567c3ce | 113 | rot[i * 3 + 0] = ubc[i]; |
114 | rot[i * 3 + 1] = uab[i]; | |
faf80567 | 115 | rot[i * 3 + 2] = n[i]; |
116 | } | |
117 | return kTRUE; | |
118 | } | |
119 | ||
faf80567 | 120 | //____________________________________________________________________ |
f567c3ce | 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 | |
faf80567 | 127 | { |
f567c3ce | 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)); | |
faf80567 | 132 | return kFALSE; |
133 | } | |
f567c3ce | 134 | |
faf80567 | 135 | TGraph2DErrors g; |
b2e6f0b0 | 136 | // Loop and fill graph |
f567c3ce | 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)); | |
b2e6f0b0 | 140 | |
f567c3ce | 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 | ||
b2e6f0b0 | 147 | // Check that we have enough points |
148 | if (g.GetN() < 4) { | |
f567c3ce | 149 | AliError(Form("Only got %d survey points - no good for plane fit", |
150 | g.GetN())); | |
b2e6f0b0 | 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 | // | |
f567c3ce | 163 | TF2 f("plane", "-[0]*x-[1]*y-[2]", |
164 | g.GetXmin(), g.GetXmax(), g.GetYmin(), g.GetYmax()); | |
b2e6f0b0 | 165 | g.Fit(&f, "Q"); |
f567c3ce | 166 | |
b2e6f0b0 | 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); | |
f567c3ce | 171 | |
b2e6f0b0 | 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()); | |
f567c3ce | 177 | // Double_t angAb = ua.Angle(ub); |
faf80567 | 178 | // PrintVector("ua: ", ua); |
179 | // PrintVector("ub: ", ub); | |
180 | // std::cout << "Angle: " << angAb * 180 / TMath::Pi() << std::endl; | |
f567c3ce | 181 | |
b2e6f0b0 | 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 | } | |
f567c3ce | 187 | |
b2e6f0b0 | 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; | |
f567c3ce | 192 | |
b2e6f0b0 | 193 | return kTRUE; |
194 | } | |
195 | ||
f567c3ce | 196 | |
b2e6f0b0 | 197 | //____________________________________________________________________ |
198 | Bool_t | |
f567c3ce | 199 | AliFMDSurveyToAlignObjs::MakeDelta(const char* path, |
200 | Double_t* rot, | |
201 | Double_t* trans, | |
202 | TGeoHMatrix& delta) const | |
b2e6f0b0 | 203 | { |
f567c3ce | 204 | // Make delta transformation |
205 | if (!gGeoManager) return kFALSE; | |
206 | if (!gGeoManager->cd(path)) return kFALSE; | |
b2e6f0b0 | 207 | |
b2e6f0b0 | 208 | |
f567c3ce | 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 | |
b2e6f0b0 | 214 | |
f567c3ce | 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 | |
b2e6f0b0 | 226 | TGeoHMatrix* geoM = new TGeoHMatrix; |
b2e6f0b0 | 227 | geoM->SetTranslation(trans); |
f567c3ce | 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; | |
b2e6f0b0 | 270 | #endif |
f567c3ce | 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; | |
b2e6f0b0 | 292 | |
f567c3ce | 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; | |
b2e6f0b0 | 313 | |
f567c3ce | 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); | |
b2e6f0b0 | 351 | |
f567c3ce | 352 | // TGeoHMatrix delta; |
353 | if (!MakeDelta("/ALIC_1/F2MT_2/FMD2_support_0/FMD2_back_cover_2", | |
354 | rot, trans, fFMD2Delta)) return kFALSE; | |
b2e6f0b0 | 355 | |
f567c3ce | 356 | // PrintRotation("FMD2 delta rotation:", fFMD2Delta.GetRotationMatrix()); |
357 | // PrintVector("FMD2 delta translation:", fFMD2Delta.GetTranslation()); | |
358 | ||
b2e6f0b0 | 359 | return kTRUE; |
360 | } | |
361 | ||
362 | //____________________________________________________________________ | |
363 | void | |
364 | AliFMDSurveyToAlignObjs::Run() | |
365 | { | |
366 | AliFMDGeometry* geom = AliFMDGeometry::Instance(); | |
367 | geom->Init(); | |
368 | geom->InitTransformations(); | |
369 | ||
f567c3ce | 370 | DoFMD1(); |
b2e6f0b0 | 371 | DoFMD2(); |
372 | } | |
373 | ||
f567c3ce | 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 | ||
b2e6f0b0 | 394 | //____________________________________________________________________ |
395 | void | |
396 | AliFMDSurveyToAlignObjs::PrintVector(const char* text, const TVector3& v) | |
397 | { | |
f567c3ce | 398 | // Print a vector |
b2e6f0b0 | 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 | { | |
f567c3ce | 406 | // Print a vector |
b2e6f0b0 | 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 | { | |
f567c3ce | 419 | // Print a rotation matrix |
b2e6f0b0 | 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 | // |