Newer
Older
TB_Chris / TbAlignment / src / .svn / text-base / TbMillepede.cpp.svn-base
  1. #include <iomanip>
  2. #include <algorithm>
  3. #include <cmath>
  4.  
  5. // Local
  6. #include "TbMillepede.h"
  7.  
  8. // TbKernel
  9. #include "TbKernel/TbGeomFunctions.h"
  10.  
  11. DECLARE_TOOL_FACTORY(TbMillepede)
  12.  
  13. //=============================================================================
  14. // Standard constructor, initializes variables
  15. //=============================================================================
  16. TbMillepede::TbMillepede(const std::string& type, const std::string& name,
  17. const IInterface* parent)
  18. : TbAlignmentBase(type, name, parent),
  19. m_nalc(4),
  20. m_debug(false),
  21. m_iterate(true) {
  22.  
  23. declareProperty("NIterations", m_nIterations = 5);
  24. declareProperty("ResCut", m_rescut = 0.05);
  25. declareProperty("ResCutInit", m_rescut_init = 0.6);
  26. declareProperty("NStdDev", m_nstdev = 0);
  27. declareProperty("Sigmas", m_sigmas = {});
  28.  
  29. m_dofsDefault = {true, true, false, true, true, true};
  30. }
  31.  
  32. //=============================================================================
  33. // Destructor
  34. //=============================================================================
  35. TbMillepede::~TbMillepede() {}
  36.  
  37. //=============================================================================
  38. // Initialization
  39. //=============================================================================
  40. StatusCode TbMillepede::initialize() {
  41.  
  42. // Initialise the base class.
  43. StatusCode sc = TbAlignmentBase::initialize();
  44. if (sc.isFailure()) return sc;
  45.  
  46. // Get the verbosity level.
  47. m_debug = msgLevel(MSG::DEBUG);
  48.  
  49. // Renumber the planes in Millepede, ignoring masked planes.
  50. m_millePlanes.assign(m_nPlanes, 999);
  51. unsigned int index = 0;
  52. for (unsigned int i = 0; i < m_nPlanes; ++i) {
  53. if (masked(i)) continue;
  54. m_millePlanes[i] = index;
  55. ++index;
  56. }
  57. // Use default values for the sigmas, unless specified explicitly.
  58. if (m_sigmas.size() != 6) {
  59. m_sigmas = {0.05, 0.05, 0.5, 0.005, 0.005, 0.005};
  60. }
  61. return StatusCode::SUCCESS;
  62. }
  63.  
  64. //=============================================================================
  65. // Main alignment function
  66. //=============================================================================
  67. void TbMillepede::align(std::vector<TbAlignmentTrack*>& alignmentTracks) {
  68.  
  69. info() << "Millepede alignment" << endmsg;
  70. const unsigned int nPlanes = m_nPlanes - m_maskedPlanes.size();
  71. const unsigned int nParameters = 6 * nPlanes;
  72. for (unsigned int iteration = 0; iteration < m_nIterations; ++iteration) {
  73. const unsigned int nTracks = alignmentTracks.size();
  74. // Define the constraint equations.
  75. setConstraints(nPlanes);
  76. const double startfact = 100.;
  77. // Initialise all matrices and vectors.
  78. reset(nPlanes, startfact);
  79. info() << "Feeding Millepede with " << nTracks << " tracks..." << endmsg;
  80. // Feed Millepede with tracks.
  81. unsigned int nSkipped = 0;
  82. unsigned int nOutliers = 0;
  83. for (unsigned int i = 0; i < nTracks; ++i) {
  84. LHCb::TbTrack* track = alignmentTracks[i]->track();
  85. if (track->size() != nPlanes) {
  86. ++nSkipped;
  87. continue;
  88. }
  89. if (!putTrack(track, nPlanes)) {
  90. ++nOutliers;
  91. }
  92. }
  93. if (nSkipped > 0) {
  94. info() << "Skipped " << nSkipped << " tracks with less than " << nPlanes
  95. << " clusters." << endmsg;
  96. }
  97. if (nOutliers > 0) {
  98. info() << "Rejected " << nOutliers << " outlier tracks." << endmsg;
  99. }
  100. // Do the global fit.
  101. info() << "Determining global parameters..." << endmsg;
  102. fitGlobal();
  103. // Calculate the convergence metric (sum of misalignments).
  104. double converg = 0.;
  105. for (unsigned int i = 0; i < nParameters; ++i) {
  106. converg += fabs(m_dparm[i]);
  107. }
  108. converg /= nParameters;
  109. info() << "Convergence: " << converg << endmsg;
  110. // Update the module positions and orientations.
  111. info() << "Updating geometry..." << endmsg;
  112. updateGeometry();
  113. // Update the cluster coordinates based on the new geometry.
  114. for (unsigned int i = 0; i < nTracks; ++i) {
  115. LHCb::TbTrack* track = alignmentTracks[i]->track();
  116. SmartRefVector<LHCb::TbCluster> clusters = track->clusters();
  117. for (auto ic = clusters.begin(), end = clusters.end(); ic != end; ++ic) {
  118. Gaudi::XYZPoint pLocal((*ic)->xloc(), (*ic)->yloc(), 0.);
  119. const auto pGlobal = geomSvc()->localToGlobal(pLocal, (*ic)->plane());
  120. (*ic)->setX(pGlobal.x());
  121. (*ic)->setY(pGlobal.y());
  122. (*ic)->setZ(pGlobal.z());
  123. }
  124. }
  125. if (converg < 0.00001) break;
  126. }
  127. }
  128.  
  129. //=============================================================================
  130. // Setup the constraint equations.
  131. //=============================================================================
  132. void TbMillepede::setConstraints(const unsigned int nPlanes) {
  133.  
  134. // Calculate the mean z-position.
  135. double avgz = 0.;
  136. for (auto im = m_modules.cbegin(), end = m_modules.cend(); im != end; ++im) {
  137. if (masked(im - m_modules.cbegin())) continue;
  138. avgz += (*im)->z();
  139. }
  140. avgz /= nPlanes;
  141. // Calculate the variance.
  142. double varz = 0.0;
  143. for (auto im = m_modules.cbegin(), end = m_modules.cend(); im != end; ++im) {
  144. if (masked(im - m_modules.cbegin())) continue;
  145. const double dz = (*im)->z() - avgz;
  146. varz += dz * dz;
  147. }
  148. varz /= nPlanes;
  149.  
  150. // Define the 9 constraints equations according to the requested geometry.
  151. const unsigned int nParameters = 6 * nPlanes;
  152. std::vector<double> ftx(nParameters, 0.);
  153. std::vector<double> fty(nParameters, 0.);
  154. std::vector<double> ftz(nParameters, 0.);
  155. std::vector<double> frx(nParameters, 0.);
  156. std::vector<double> fry(nParameters, 0.);
  157. std::vector<double> frz(nParameters, 0.);
  158. std::vector<double> fscaz(nParameters, 0.);
  159. std::vector<double> shearx(nParameters, 0.);
  160. std::vector<double> sheary(nParameters, 0.);
  161.  
  162. m_constraints.clear();
  163. for (auto im = m_modules.cbegin(), end = m_modules.cend(); im != end; ++im) {
  164. if (masked(im - m_modules.begin())) continue;
  165. const unsigned int i = m_millePlanes[im - m_modules.begin()];
  166. const double sz = ((*im)->z() - avgz) / varz;
  167. ftx[i] = 1.0;
  168. fty[i + nPlanes] = 1.0;
  169. ftz[i + 2 * nPlanes] = 1.0;
  170. frx[i + 3 * nPlanes] = 1.0 ; // i > 4 ? 1.0 : -1.0;
  171. fry[i + 4 * nPlanes] = 1.0 ; // i > 4 ? 1.0 : -1.0;
  172. frz[i + 5 * nPlanes] = 1.0;
  173. shearx[i] = sz;
  174. sheary[i + nPlanes] = sz;
  175. fscaz[i + 2 * nPlanes] = sz;
  176. }
  177.  
  178. const std::vector<bool> constraints = {true, true, true, true, false,
  179. false, true, true, true};
  180. // Put the constraints information in the basket.
  181. if (constraints[0] && m_dofs[0]) addConstraint(ftx, 0.0);
  182. if (constraints[1] && m_dofs[0]) addConstraint(shearx, 0.);
  183. if (constraints[2] && m_dofs[1]) addConstraint(fty, 0.0);
  184. if (constraints[3] && m_dofs[1]) addConstraint(sheary, 0.);
  185. if (constraints[4] && m_dofs[2]) addConstraint(ftz, 0.0);
  186. // if (constraints[5] && m_dofs[2]) addConstraint(fscaz, 0.0);
  187. if (constraints[6] && m_dofs[3]) addConstraint(frx, 0.0);
  188. if (constraints[7] && m_dofs[4]) addConstraint(fry, 0.0);
  189. if (constraints[8] && m_dofs[5]) addConstraint(frz, 0.0);
  190. }
  191.  
  192. //=============================================================================
  193. // Define a single constraint equation.
  194. //=============================================================================
  195. void TbMillepede::addConstraint(const std::vector<double>& dercs,
  196. const double rhs) {
  197.  
  198. Constraint constraint;
  199. // Set the right-hand side (Lagrange multiplier value, sum of equation).
  200. constraint.rhs = rhs;
  201. // Set the constraint equation coefficients.
  202. constraint.coefficients = dercs;
  203. m_constraints.push_back(constraint);
  204. }
  205.  
  206. //=============================================================================
  207. // Add the equations for one track to the matrix
  208. //=============================================================================
  209. bool TbMillepede::putTrack(LHCb::TbTrack* track, const unsigned int nPlanes) {
  210.  
  211. std::vector<Equation> equations;
  212. const unsigned int nParameters = 6 * nPlanes;
  213. // Global derivatives
  214. std::vector<double> dergb(nParameters, 0.);
  215. // Global derivatives non linearly related to residual
  216. std::vector<double> dernl(nParameters, 0.);
  217. // Local parameter indices associated with non-linear derivatives
  218. std::vector<int> dernli(nParameters, 0);
  219. // Track slopes
  220. std::vector<double> dernls(nParameters, 0.);
  221.  
  222. /// Refit the track for the reference states.
  223. m_trackFit->fit(track);
  224. auto state = track->firstState();
  225. const double tx = state.tx();
  226. const double ty = state.ty();
  227.  
  228. // Iterate over each cluster on the track.
  229. const SmartRefVector<LHCb::TbCluster>& clusters = track->clusters();
  230. for (auto itc = clusters.cbegin(), end = clusters.cend(); itc != end; ++itc) {
  231. if (masked((*itc)->plane())) continue;
  232. const auto normal = geomSvc()->module((*itc)->plane())->normal();
  233. double nx = normal.x() / normal.z();
  234. double ny = normal.y() / normal.z();
  235. const double xg = (*itc)->x();
  236. const double yg = (*itc)->y();
  237. const double zg = (*itc)->z();
  238. // Calculate quasi-local coordinates.
  239. const double zl = zg - m_modules[(*itc)->plane()]->z();
  240. const double xl = xg - m_modules[(*itc)->plane()]->x();
  241. const double yl = yg - m_modules[(*itc)->plane()]->y();
  242.  
  243. std::vector<double> nonlinear = {
  244. nx, ny, 1., -yl + zl * ny, -xl + zl * nx, xl * ny - yl * nx};
  245. const double den = 1. + tx * nx + ty * ny;
  246. for (auto& a : nonlinear) a /= den;
  247. // Get the errors on the measured x and y coordinates.
  248. const double errx = (*itc)->xErr();
  249. const double erry = (*itc)->yErr();
  250. // Get the internal plane index in Millepede.
  251. const unsigned int plane = m_millePlanes[(*itc)->plane()];
  252. // Set the local derivatives for the X equation.
  253. std::vector<double> derlc = {1., zg, 0., 0.};
  254. // Set the global derivatives (see LHCb-2005-101) for the X equation.
  255. std::fill(dergb.begin(), dergb.end(), 0.);
  256. std::fill(dernl.begin(), dernl.end(), 0.);
  257. std::fill(dernli.begin(), dernli.end(), 0);
  258. std::fill(dernls.begin(), dernls.end(), 0.);
  259. if (m_dofs[0]) dergb[plane] = -1.;
  260. if (m_dofs[4]) dergb[4 * nPlanes + plane] = -zl;
  261. if (m_dofs[5]) dergb[5 * nPlanes + plane] = yl;
  262. for (unsigned int i = 0; i < 6; ++i) {
  263. if (!m_dofs[i]) continue;
  264. dergb[i * nPlanes + plane] += tx * nonlinear[i];
  265. dernl[i * nPlanes + plane] = nonlinear[i];
  266. dernli[i * nPlanes + plane] = 1;
  267. dernls[i * nPlanes + plane] = tx;
  268. }
  269. // Store the X equation.
  270. addEquation(equations, derlc, dergb, dernl, dernli, dernls, xg, errx);
  271. // Set the local derivatives for the Y equation.
  272. derlc = {0., 0., 1., zg};
  273. // Set the global derivatives (see LHCb-2005-101) for the Y equation.
  274. std::fill(dergb.begin(), dergb.end(), 0.);
  275. std::fill(dernl.begin(), dernl.end(), 0.);
  276. std::fill(dernli.begin(), dernli.end(), 0);
  277. std::fill(dernls.begin(), dernls.end(), 0.);
  278. if (m_dofs[1]) dergb[nPlanes + plane] = -1.;
  279. if (m_dofs[3]) dergb[3 * nPlanes + plane] = -zl;
  280. if (m_dofs[5]) dergb[5 * nPlanes + plane] = -xl;
  281. for (unsigned int i = 0; i < 6; ++i) {
  282. if (!m_dofs[i]) continue;
  283. dergb[i * nPlanes + plane] += ty * nonlinear[i];
  284. dernl[i * nPlanes + plane] = nonlinear[i];
  285. dernli[i * nPlanes + plane] = 3;
  286. dernls[i * nPlanes + plane] = ty;
  287. }
  288. // Store the Y equation.
  289. addEquation(equations, derlc, dergb, dernl, dernli, dernls, yg, erry);
  290. }
  291. // Vector containing the track parameters
  292. std::vector<double> trackParams(2 * m_nalc + 2, 0.);
  293. // Fit the track.
  294. const unsigned int iteration = 1;
  295. const bool ok = fitTrack(equations, trackParams, false, iteration);
  296. if (ok) m_equations.push_back(equations);
  297. return ok;
  298. }
  299.  
  300. //=============================================================================
  301. // Store the parameters for one measurement
  302. //=============================================================================
  303. void TbMillepede::addEquation(std::vector<Equation>& equations,
  304. const std::vector<double>& derlc,
  305. const std::vector<double>& dergb,
  306. const std::vector<double>& dernl,
  307. const std::vector<int>& dernli,
  308. const std::vector<double>& slopes,
  309. const double rmeas, const double sigma) {
  310.  
  311. if (sigma <= 0.) {
  312. error() << "Invalid cluster error (" << sigma << ")" << endmsg;
  313. return;
  314. }
  315. Equation equation;
  316. equation.rmeas = rmeas;
  317. equation.weight = 1. / (sigma * sigma);
  318. // Add non-zero local derivatives and corresponding indices.
  319. equation.derL.clear();
  320. equation.indL.clear();
  321. for (unsigned int i = 0; i < m_nalc; ++i) {
  322. if (derlc[i] == 0.) continue;
  323. equation.indL.push_back(i);
  324. equation.derL.push_back(derlc[i]);
  325. }
  326. // Add non-zero global derivatives and corresponding indices.
  327. equation.derG.clear();
  328. equation.indG.clear();
  329. equation.derNL.clear();
  330. equation.indNL.clear();
  331. equation.slopes.clear();
  332. const unsigned int nG = dergb.size();
  333. for (unsigned int i = 0; i < nG; ++i) {
  334. if (dergb[i] == 0.) continue;
  335. equation.indG.push_back(i);
  336. equation.derG.push_back(dergb[i]);
  337. equation.derNL.push_back(dernl[i]);
  338. equation.indNL.push_back(dernli[i]);
  339. equation.slopes.push_back(slopes[i]);
  340. }
  341. // Add the equation to the list.
  342. equations.push_back(std::move(equation));
  343. }
  344.  
  345. //=============================================================================
  346. // Track fit (local fit)
  347. //=============================================================================
  348. bool TbMillepede::fitTrack(const std::vector<Equation>& equations,
  349. std::vector<double>& trackParams,
  350. const bool singlefit, const unsigned int iteration) {
  351.  
  352. std::vector<double> blvec(m_nalc, 0.);
  353. std::vector<std::vector<double> > clmat(m_nalc,
  354. std::vector<double>(m_nalc, 0.));
  355.  
  356. // First loop: local track fit
  357. for (const auto& equation : equations) {
  358. double rmeas = equation.rmeas;
  359. // Suppress the global part (only relevant with iterations).
  360. const unsigned int nG = equation.derG.size();
  361. for (unsigned int i = 0; i < nG; ++i) {
  362. const unsigned int j = equation.indG[i];
  363. rmeas -= equation.derG[i] * m_dparm[j];
  364. }
  365. const double w = equation.weight;
  366. // Fill local matrix and vector.
  367. const unsigned int nL = equation.derL.size();
  368. for (unsigned int i = 0; i < nL; ++i) {
  369. const unsigned int j = equation.indL[i];
  370. blvec[j] += w * rmeas * equation.derL[i];
  371. if (m_debug) debug() << "blvec[" << j << "] = " << blvec[j] << endmsg;
  372. // Symmetric matrix, don't bother k > j coefficients.
  373. for (unsigned int k = 0; k <= i; ++k) {
  374. const unsigned int ik = equation.indL[k];
  375. clmat[j][ik] += w * equation.derL[i] * equation.derL[k];
  376. if (m_debug) {
  377. debug() << "clmat[" << j << "][" << ik << "] = " << clmat[j][ik]
  378. << endmsg;
  379. }
  380. }
  381. }
  382. }
  383.  
  384. // Local parameter matrix is completed, now invert to solve.
  385. // Rank: number of non-zero diagonal elements.
  386. int rank = invertMatrixLocal(clmat, blvec, m_nalc);
  387. // Store the track parameters and errors.
  388. for (unsigned int i = 0; i < m_nalc; ++i) {
  389. trackParams[2 * i] = blvec[i];
  390. trackParams[2 * i + 1] = sqrt(fabs(clmat[i][i]));
  391. }
  392.  
  393. // Second loop: residual calculation.
  394. double chi2 = 0.0;
  395. int ndf = 0;
  396. for (const auto& equation : equations) {
  397. double rmeas = equation.rmeas;
  398. // Suppress global and local terms.
  399. const unsigned int nL = equation.derL.size();
  400. for (unsigned int i = 0; i < nL; ++i) {
  401. const unsigned int j = equation.indL[i];
  402. rmeas -= equation.derL[i] * blvec[j];
  403. }
  404. const unsigned int nG = equation.derG.size();
  405. for (unsigned int i = 0; i < nG; ++i) {
  406. const unsigned int j = equation.indG[i];
  407. rmeas -= equation.derG[i] * m_dparm[j];
  408. }
  409. // Now rmeas contains the residual value.
  410. if (m_debug) debug() << "Residual value: " << rmeas << endmsg;
  411. // Reject the track if the residual is too large (outlier).
  412. const double rescut = iteration <= 1 ? m_rescut_init : m_rescut;
  413. if (fabs(rmeas) >= rescut) {
  414. if (m_debug) {
  415. debug() << "Reject track due to residual cut in iteration " << iteration
  416. << endmsg;
  417. }
  418. return false;
  419. }
  420. chi2 += equation.weight * rmeas * rmeas;
  421. ++ndf;
  422. }
  423. ndf -= rank;
  424. const bool printDetails = false;
  425. if (printDetails) {
  426. info() << endmsg;
  427. info() << "Local track fit (rank " << rank << ")" << endmsg;
  428. info() << " Result of local fit : (index/parameter/error)" << endmsg;
  429. for (unsigned int i = 0; i < m_nalc; ++i) {
  430. info() << std::setprecision(6) << std::fixed << std::setw(20) << i
  431. << " / " << std::setw(10) << blvec[i] << " / "
  432. << sqrt(clmat[i][i]) << endmsg;
  433. }
  434. info() << "Final chi square / degrees of freedom: " << chi2 << " / " << ndf
  435. << endmsg;
  436. }
  437.  
  438. // Stop here if just updating the track parameters.
  439. if (singlefit) return true;
  440.  
  441. if (m_nstdev != 0 && ndf > 0) {
  442. const double chi2PerNdf = chi2 / ndf;
  443. const double cut = chi2Limit(m_nstdev, ndf) * m_cfactr;
  444. if (chi2 > cut) {
  445. // Reject the track.
  446. if (m_debug) {
  447. debug() << "Rejected track because chi2 / ndof (" << chi2PerNdf
  448. << ") is larger than " << cut << endmsg;
  449. }
  450. return false;
  451. }
  452. }
  453. // Store the chi2 and number of degrees of freedom.
  454. trackParams[2 * m_nalc] = ndf;
  455. trackParams[2 * m_nalc + 1] = chi2;
  456.  
  457. // Local operations are finished. Track is accepted.
  458. // Third loop: update the global parameters (other matrices).
  459. m_clcmat.assign(m_nagb, std::vector<double>(m_nalc, 0.));
  460. unsigned int nagbn = 0;
  461. std::vector<int> indnz(m_nagb, -1);
  462. std::vector<int> indbk(m_nagb, 0);
  463. for (const auto& equation : equations) {
  464. double rmeas = equation.rmeas;
  465. // Suppress the global part.
  466. const unsigned int nG = equation.derG.size();
  467. for (unsigned int i = 0; i < nG; ++i) {
  468. const unsigned int j = equation.indG[i];
  469. rmeas -= equation.derG[i] * m_dparm[j];
  470. }
  471. const double w = equation.weight;
  472. // First of all, the global/global terms.
  473. for (unsigned int i = 0; i < nG; ++i) {
  474. const unsigned int j = equation.indG[i];
  475. m_bgvec[j] += w * rmeas * equation.derG[i];
  476. if (m_debug) debug() << "bgvec[" << j << "] = " << m_bgvec[j] << endmsg;
  477. for (unsigned int k = 0; k < nG; ++k) {
  478. const unsigned int n = equation.indG[k];
  479. m_cgmat[j][n] += w * equation.derG[i] * equation.derG[k];
  480. if (m_debug) {
  481. debug() << "cgmat[" << j << "][" << n << "] = " << m_cgmat[j][n]
  482. << endmsg;
  483. }
  484. }
  485. }
  486. // Now we have also rectangular matrices containing global/local terms.
  487. const unsigned int nL = equation.derL.size();
  488. for (unsigned int i = 0; i < nG; ++i) {
  489. const unsigned int j = equation.indG[i];
  490. // Index of index.
  491. int ik = indnz[j];
  492. if (ik == -1) {
  493. // New global variable.
  494. indnz[j] = nagbn;
  495. indbk[nagbn] = j;
  496. ik = nagbn;
  497. ++nagbn;
  498. }
  499. // Now fill the rectangular matrix.
  500. for (unsigned int k = 0; k < nL; ++k) {
  501. const unsigned int ij = equation.indL[k];
  502. m_clcmat[ik][ij] += w * equation.derG[i] * equation.derL[k];
  503. if (m_debug)
  504. debug() << "clcmat[" << ik << "][" << ij << "] = " << m_clcmat[ik][ij]
  505. << endmsg;
  506. }
  507. }
  508. }
  509.  
  510. // Third loop is finished, now we update the correction matrices.
  511. multiplyAVAt(clmat, m_clcmat, m_corrm, m_nalc, nagbn);
  512. multiplyAX(m_clcmat, blvec, m_corrv, m_nalc, nagbn);
  513. for (unsigned int i = 0; i < nagbn; ++i) {
  514. const unsigned int j = indbk[i];
  515. m_bgvec[j] -= m_corrv[i];
  516. for (unsigned int k = 0; k < nagbn; ++k) {
  517. const unsigned int ik = indbk[k];
  518. m_cgmat[j][ik] -= m_corrm[i][k];
  519. }
  520. }
  521. return true;
  522. }
  523.  
  524. //=============================================================================
  525. // Update the module positions and orientations.
  526. //=============================================================================
  527. void TbMillepede::updateGeometry() {
  528.  
  529. const unsigned int nPlanes = m_nPlanes - m_maskedPlanes.size();
  530. for (auto im = m_modules.cbegin(), end = m_modules.cend(); im != end; ++im) {
  531. if (masked(im - m_modules.begin())) continue;
  532. const unsigned int plane = m_millePlanes[im - m_modules.begin()];
  533. const double tx = (*im)->x() + m_dparm[plane + 0 * nPlanes];
  534. const double ty = (*im)->y() + m_dparm[plane + 1 * nPlanes];
  535. const double tz = (*im)->z() + m_dparm[plane + 2 * nPlanes];
  536. double rx = 0.;
  537. double ry = 0.;
  538. double rz = 0.;
  539. Tb::SmallRotation((*im)->rotX(), m_dparm[plane + 3 * nPlanes],
  540. (*im)->rotY(), m_dparm[plane + 4 * nPlanes],
  541. m_dparm[plane + 5 * nPlanes], rx, ry, rz);
  542. (*im)->setAlignment(tx, ty, tz, (*im)->rotX() - rx, (*im)->rotY() + ry,
  543. (*im)->rotZ() - rz, 0., 0., 0., 0., 0., 0.);
  544. }
  545. }
  546.  
  547. //=============================================================================
  548. // Initialise the vectors and arrays.
  549. //=============================================================================
  550. bool TbMillepede::reset(const unsigned int nPlanes, const double startfact) {
  551.  
  552. info() << " " << endmsg;
  553. info() << " * o o o " << endmsg;
  554. info() << " o o o " << endmsg;
  555. info() << " o ooooo o o o oo ooo oo ooo oo " << endmsg;
  556. info() << " o o o o o o o o o o o o o o o o " << endmsg;
  557. info() << " o o o o o o oooo o o oooo o o oooo " << endmsg;
  558. info() << " o o o o o o o ooo o o o o " << endmsg;
  559. info() << " o o o o o o oo o oo ooo oo ++ starts" << endmsg;
  560. info() << " " << endmsg;
  561.  
  562. // Reset the list of track equations.
  563. m_equations.clear();
  564. // Set the number of global derivatives.
  565. m_nagb = 6 * nPlanes;
  566. // Reset matrices and vectors.
  567. const unsigned int nRows = m_nagb + m_constraints.size();
  568. m_bgvec.resize(nRows, 0.);
  569. m_cgmat.assign(nRows, std::vector<double>(nRows, 0.));
  570. m_corrv.assign(m_nagb, 0.);
  571. m_corrm.assign(m_nagb, std::vector<double>(m_nagb, 0.));
  572. m_dparm.assign(m_nagb, 0.);
  573.  
  574. // Define the sigmas for each parameter.
  575. m_fixed.assign(m_nagb, true);
  576. m_psigm.assign(m_nagb, 0.);
  577. for (unsigned int i = 0; i < 6; ++i) {
  578. if (!m_dofs[i]) continue;
  579. for (unsigned int j = i * nPlanes; j < (i + 1) * nPlanes; ++j) {
  580. m_fixed[j] = false;
  581. m_psigm[j] = m_sigmas[i];
  582. }
  583. }
  584. // Fix modules if requested.
  585. for (auto it = m_fixedPlanes.cbegin(); it != m_fixedPlanes.cend(); ++it) {
  586. info() << "You are fixing module " << (*it) << endmsg;
  587. // TODO: check if this is the "millePlanes" index or the regular one.
  588. const unsigned ndofs = m_fix_all ? 6 : 3;
  589. for (unsigned int i = 0; i < ndofs; ++i) {
  590. m_fixed[(*it) + i * nPlanes] = true;
  591. m_psigm[(*it) + i * nPlanes] = 0.;
  592. }
  593. }
  594.  
  595. // Set the chi2 / ndof cut used in the local track fit.
  596. // Iterations are stopped when the cut factor reaches the ref. value (1).
  597. m_cfactref = 1.;
  598. m_cfactr = std::max(1., startfact);
  599.  
  600. return true;
  601. }
  602.  
  603. //=============================================================================
  604. //
  605. //=============================================================================
  606. bool TbMillepede::fitGlobal() {
  607.  
  608. m_diag.assign(m_nagb, 0.);
  609. std::vector<double> bgvecPrev(m_nagb, 0.);
  610. std::vector<double> trackParams(2 * m_nalc + 2, 0.);
  611. const unsigned int nTracks = m_equations.size();
  612. std::vector<std::vector<double> > localParams(
  613. nTracks, std::vector<double>(m_nalc, 0.));
  614.  
  615. const unsigned int nMaxIterations = 10;
  616. unsigned int iteration = 1;
  617. unsigned int nGoodTracks = nTracks;
  618. while (iteration <= nMaxIterations) {
  619. info() << "Iteration " << iteration << " (using " << nGoodTracks
  620. << " tracks)" << endmsg;
  621.  
  622. // Save the diagonal elements.
  623. for (unsigned int i = 0; i < m_nagb; ++i) {
  624. m_diag[i] = m_cgmat[i][i];
  625. }
  626.  
  627. unsigned int nFixed = 0;
  628. for (unsigned int i = 0; i < m_nagb; ++i) {
  629. if (m_fixed[i]) {
  630. // Fixed global parameter.
  631. ++nFixed;
  632. for (unsigned int j = 0; j < m_nagb; ++j) {
  633. // Reset row and column.
  634. m_cgmat[i][j] = 0.0;
  635. m_cgmat[j][i] = 0.0;
  636. }
  637. } else {
  638. m_cgmat[i][i] += 1. / (m_psigm[i] * m_psigm[i]);
  639. }
  640. }
  641. // Add the constraints equations.
  642. unsigned int nRows = m_nagb;
  643. for (const auto& constraint : m_constraints) {
  644. double sum = constraint.rhs;
  645. for (unsigned int j = 0; j < m_nagb; ++j) {
  646. if (m_psigm[j] == 0.) {
  647. m_cgmat[nRows][j] = 0.0;
  648. m_cgmat[j][nRows] = 0.0;
  649. } else {
  650. m_cgmat[nRows][j] = nGoodTracks * constraint.coefficients[j];
  651. m_cgmat[j][nRows] = m_cgmat[nRows][j];
  652. }
  653. sum -= constraint.coefficients[j] * m_dparm[j];
  654. }
  655. m_cgmat[nRows][nRows] = 0.0;
  656. m_bgvec[nRows] = nGoodTracks * sum;
  657. ++nRows;
  658. }
  659.  
  660. double cor = 0.0;
  661. if (iteration > 1) {
  662. for (unsigned int j = 0; j < m_nagb; ++j) {
  663. for (unsigned int i = 0; i < m_nagb; ++i) {
  664. if (m_fixed[i]) continue;
  665. cor += bgvecPrev[j] * m_cgmat[j][i] * bgvecPrev[i];
  666. if (i == j) {
  667. cor -= bgvecPrev[i] * bgvecPrev[i] / (m_psigm[i] * m_psigm[i]);
  668. }
  669. }
  670. }
  671. }
  672. if (m_debug) debug() << " Final corr. is " << cor << endmsg;
  673.  
  674. // Do the matrix inversion.
  675. const int rank = invertMatrix(m_cgmat, m_bgvec, nRows);
  676. // Update the global parameters values.
  677. for (unsigned int i = 0; i < m_nagb; ++i) {
  678. m_dparm[i] += m_bgvec[i];
  679. bgvecPrev[i] = m_bgvec[i];
  680. if (m_debug) {
  681. debug() << "bgvec[" << i << "] = " << m_bgvec[i] << endmsg;
  682. debug() << "dparm[" << i << "] = " << m_dparm[i] << endmsg;
  683. debug() << "cgmat[" << i << "][" << i << "] = " << m_cgmat[i][i]
  684. << endmsg;
  685. debug() << "err = " << sqrt(fabs(m_cgmat[i][i])) << endmsg;
  686. debug() << "cgmat * diag = " << std::setprecision(5)
  687. << m_cgmat[i][i] * m_diag[i] << endmsg;
  688. }
  689. }
  690. if (nRows - nFixed - rank != 0) {
  691. warning() << "The rank defect of the symmetric " << nRows << " by "
  692. << nRows << " matrix is " << nRows - nFixed - rank << endmsg;
  693. }
  694. if (!m_iterate) break;
  695. ++iteration;
  696. if (iteration == nMaxIterations) break;
  697. // Update the factor in the track chi2 cut.
  698. const double newcfactr = sqrt(m_cfactr);
  699. if (newcfactr > 1.2 * m_cfactref) {
  700. m_cfactr = newcfactr;
  701. } else {
  702. m_cfactr = m_cfactref;
  703. }
  704. if (m_debug) {
  705. debug() << "Refitting tracks with cut factor " << m_cfactr << endmsg;
  706. }
  707.  
  708. // Reset global variables.
  709. for (unsigned int i = 0; i < nRows; ++i) {
  710. m_bgvec[i] = 0.0;
  711. for (unsigned int j = 0; j < nRows; ++j) m_cgmat[i][j] = 0.0;
  712. }
  713. // Refit the tracks.
  714. double chi2 = 0.;
  715. double ndof = 0.;
  716. nGoodTracks = 0;
  717. for (unsigned int i = 0; i < nTracks; ++i) {
  718. // Skip invalidated tracks.
  719. if (m_equations[i].empty()) continue;
  720. std::vector<Equation> equations(m_equations[i].begin(),
  721. m_equations[i].end());
  722. for (auto equation : equations) {
  723. const unsigned int nG = equation.derG.size();
  724. for (unsigned int j = 0; j < nG; ++j) {
  725. const double t = localParams[i][equation.indNL[j]];
  726. if (t == 0) continue;
  727. equation.derG[j] += equation.derNL[j] * (t - equation.slopes[j]);
  728. }
  729. }
  730. std::fill(trackParams.begin(), trackParams.end(), 0.);
  731. // Refit the track.
  732. bool ok = fitTrack(equations, trackParams, false, iteration);
  733. // Cache the track state.
  734. for (unsigned int j = 0; j < m_nalc; ++j) {
  735. localParams[i][j] = trackParams[2 * j];
  736. }
  737. if (ok) {
  738. // Update the total chi2.
  739. chi2 += trackParams[2 * m_nalc + 1];
  740. ndof += trackParams[2 * m_nalc];
  741. ++nGoodTracks;
  742. } else {
  743. // Disable the track.
  744. m_equations[i].clear();
  745. }
  746. }
  747. info() << "Chi2 / DOF after re-fit: " << chi2 / (ndof - nRows) << endmsg;
  748. }
  749.  
  750. // Print the final results.
  751. printResults();
  752.  
  753. info() << endmsg;
  754. info() << " * o o o " << endmsg;
  755. info() << " o o o " << endmsg;
  756. info() << " o ooooo o o o oo ooo oo ooo oo " << endmsg;
  757. info() << " o o o o o o o o o o o o o o o o " << endmsg;
  758. info() << " o o o o o o oooo o o oooo o o oooo " << endmsg;
  759. info() << " o o o o o o o ooo o o o o " << endmsg;
  760. info() << " o o o o o o oo o oo ooo oo ++ ends." << endmsg;
  761. info() << " o " << endmsg;
  762. info() << endmsg;
  763. return true;
  764. }
  765.  
  766. //=============================================================================
  767. // Obtain solution of a system of linear equations with symmetric matrix
  768. // and the inverse (using 'singular-value friendly' GAUSS pivot).
  769. // Solve the equation V * X = B.
  770. // V is replaced by its inverse matrix and B by X, the solution vector
  771. //=============================================================================
  772. int TbMillepede::invertMatrix(std::vector<std::vector<double> >& v,
  773. std::vector<double>& b, const int n) {
  774. int rank = 0;
  775. const double eps = 0.0000000000001;
  776.  
  777. std::vector<double> diag(n, 0.);
  778. std::vector<bool> used_param(n, true);
  779. std::vector<bool> flag(n, true);
  780. for (int i = 0; i < n; i++) {
  781. for (int j = 0; j <= i; j++) {
  782. v[j][i] = v[i][j];
  783. }
  784. }
  785.  
  786. // Find max. elements of each row and column.
  787. std::vector<double> r(n, 0.);
  788. std::vector<double> c(n, 0.);
  789. for (int i = 0; i < n; i++) {
  790. for (int j = 0; j < n; j++) {
  791. if (fabs(v[i][j]) >= r[i]) r[i] = fabs(v[i][j]);
  792. if (fabs(v[j][i]) >= c[i]) c[i] = fabs(v[j][i]);
  793. }
  794. }
  795. for (int i = 0; i < n; i++) {
  796. if (0.0 != r[i]) r[i] = 1. / r[i];
  797. if (0.0 != c[i]) c[i] = 1. / c[i];
  798. // Check if max elements are within requested precision.
  799. if (eps >= r[i]) r[i] = 0.0;
  800. if (eps >= c[i]) c[i] = 0.0;
  801. }
  802.  
  803. // Equilibrate the V matrix
  804. for (int i = 0; i < n; i++) {
  805. for (int j = 0; j < n; j++) {
  806. v[i][j] = sqrt(r[i]) * v[i][j] * sqrt(c[j]);
  807. }
  808. }
  809.  
  810. for (int i = 0; i < n; i++) {
  811. // Save the absolute values of the diagonal elements.
  812. diag[i] = fabs(v[i][i]);
  813. if (r[i] == 0. && c[i] == 0.) {
  814. // This part is empty (non-linear treatment with non constraints)
  815. flag[i] = false;
  816. used_param[i] = false;
  817. }
  818. }
  819.  
  820. for (int i = 0; i < n; i++) {
  821. double vkk = 0.0;
  822. int k = -1;
  823. // First look for the pivot, i. e. the max unused diagonal element.
  824. for (int j = 0; j < n; j++) {
  825. if (flag[j] && (fabs(v[j][j]) > std::max(fabs(vkk), eps))) {
  826. vkk = v[j][j];
  827. k = j;
  828. }
  829. }
  830.  
  831. if (k >= 0) {
  832. // Pivot found.
  833. rank++;
  834. // Use this value.
  835. flag[k] = false;
  836. // Replace pivot by its inverse.
  837. vkk = 1.0 / vkk;
  838. v[k][k] = -vkk;
  839. for (int j = 0; j < n; j++) {
  840. for (int jj = 0; jj < n; jj++) {
  841. if (j != k && jj != k && used_param[j] && used_param[jj]) {
  842. // Other elements (do them first as you use old v[k][j])
  843. v[j][jj] = v[j][jj] - vkk * v[j][k] * v[k][jj];
  844. }
  845. }
  846. }
  847. for (int j = 0; j < n; j++) {
  848. if (j != k && used_param[j]) {
  849. v[j][k] = v[j][k] * vkk;
  850. v[k][j] = v[k][j] * vkk;
  851. }
  852. }
  853. } else {
  854. // No more pivot value (clear those elements)
  855. for (int j = 0; j < n; j++) {
  856. if (flag[j]) {
  857. b[j] = 0.0;
  858. for (int k = 0; k < n; k++) {
  859. v[j][k] = 0.0;
  860. v[k][j] = 0.0;
  861. }
  862. }
  863. }
  864. break;
  865. }
  866. }
  867. // Correct matrix V
  868. for (int i = 0; i < n; i++) {
  869. for (int j = 0; j < n; j++) {
  870. v[i][j] = sqrt(c[i]) * v[i][j] * sqrt(r[j]);
  871. }
  872. }
  873.  
  874. std::vector<double> temp(n, 0.);
  875. for (int j = 0; j < n; j++) {
  876. // Reverse matrix elements
  877. for (int jj = 0; jj < n; jj++) {
  878. v[j][jj] = -v[j][jj];
  879. temp[j] += v[j][jj] * b[jj];
  880. }
  881. }
  882.  
  883. for (int j = 0; j < n; j++) {
  884. b[j] = temp[j];
  885. }
  886. return rank;
  887. }
  888.  
  889. //=============================================================================
  890. // Simplified version.
  891. //=============================================================================
  892. int TbMillepede::invertMatrixLocal(std::vector<std::vector<double> >& v,
  893. std::vector<double>& b, const int n) {
  894.  
  895. int rank = 0;
  896. const double eps = 0.0000000000001;
  897.  
  898. std::vector<bool> flag(n, true);
  899. std::vector<double> diag(n, 0.);
  900. for (int i = 0; i < n; i++) {
  901. // Save the absolute values of the diagonal elements.
  902. diag[i] = fabs(v[i][i]);
  903. for (int j = 0; j <= i; j++) {
  904. v[j][i] = v[i][j];
  905. }
  906. }
  907.  
  908. for (int i = 0; i < n; i++) {
  909. double vkk = 0.0;
  910. int k = -1;
  911. // First look for the pivot, i. e. the max. unused diagonal element.
  912. for (int j = 0; j < n; j++) {
  913. if (flag[j] && (fabs(v[j][j]) > std::max(fabs(vkk), eps * diag[j]))) {
  914. vkk = v[j][j];
  915. k = j;
  916. }
  917. }
  918.  
  919. if (k >= 0) {
  920. // Pivot found
  921. rank++;
  922. flag[k] = false;
  923. // Replace pivot by its inverse.
  924. vkk = 1.0 / vkk;
  925. v[k][k] = -vkk;
  926. for (int j = 0; j < n; j++) {
  927. for (int jj = 0; jj < n; jj++) {
  928. if (j != k && jj != k) {
  929. // Other elements (do them first as you use old v[k][j])
  930. v[j][jj] = v[j][jj] - vkk * v[j][k] * v[k][jj];
  931. }
  932. }
  933. }
  934.  
  935. for (int j = 0; j < n; j++) {
  936. if (j != k) {
  937. v[j][k] = v[j][k] * vkk;
  938. v[k][j] = v[k][j] * vkk;
  939. }
  940. }
  941. } else {
  942. // No more pivot values (clear those elements).
  943. for (int j = 0; j < n; j++) {
  944. if (flag[j]) {
  945. b[j] = 0.0;
  946. for (k = 0; k < n; k++) v[j][k] = 0.0;
  947. }
  948. }
  949. break;
  950. }
  951. }
  952.  
  953. std::vector<double> temp(n, 0.);
  954. for (int j = 0; j < n; j++) {
  955. // Reverse matrix elements
  956. for (int jj = 0; jj < n; jj++) {
  957. v[j][jj] = -v[j][jj];
  958. temp[j] += v[j][jj] * b[jj];
  959. }
  960. }
  961. for (int j = 0; j < n; j++) {
  962. b[j] = temp[j];
  963. }
  964. return rank;
  965. }
  966.  
  967. //=============================================================================
  968. // Return the limit in chi^2 / nd for n sigmas stdev authorized.
  969. // Only n=1, 2, and 3 are expected in input.
  970. //=============================================================================
  971. double TbMillepede::chi2Limit(const int n, const int nd) const {
  972. constexpr double sn[3] = {0.47523, 1.690140, 2.782170};
  973. constexpr double table[3][30] = {
  974. {1.0000, 1.1479, 1.1753, 1.1798, 1.1775, 1.1730, 1.1680, 1.1630,
  975. 1.1581, 1.1536, 1.1493, 1.1454, 1.1417, 1.1383, 1.1351, 1.1321,
  976. 1.1293, 1.1266, 1.1242, 1.1218, 1.1196, 1.1175, 1.1155, 1.1136,
  977. 1.1119, 1.1101, 1.1085, 1.1070, 1.1055, 1.1040},
  978. {4.0000, 3.0900, 2.6750, 2.4290, 2.2628, 2.1415, 2.0481, 1.9736,
  979. 1.9124, 1.8610, 1.8171, 1.7791, 1.7457, 1.7161, 1.6897, 1.6658,
  980. 1.6442, 1.6246, 1.6065, 1.5899, 1.5745, 1.5603, 1.5470, 1.5346,
  981. 1.5230, 1.5120, 1.5017, 1.4920, 1.4829, 1.4742},
  982. {9.0000, 5.9146, 4.7184, 4.0628, 3.6410, 3.3436, 3.1209, 2.9468,
  983. 2.8063, 2.6902, 2.5922, 2.5082, 2.4352, 2.3711, 2.3143, 2.2635,
  984. 2.2178, 2.1764, 2.1386, 2.1040, 2.0722, 2.0428, 2.0155, 1.9901,
  985. 1.9665, 1.9443, 1.9235, 1.9040, 1.8855, 1.8681}};
  986.  
  987. if (nd < 1) return 0.;
  988. const int m = std::max(1, std::min(n, 3));
  989. if (nd <= 30) return table[m - 1][nd - 1];
  990. return ((sn[m - 1] + sqrt(float(2 * nd - 3))) *
  991. (sn[m - 1] + sqrt(float(2 * nd - 3)))) /
  992. float(2 * nd - 2);
  993. }
  994.  
  995. //=============================================================================
  996. // Multiply general M-by-N matrix A and N-vector X
  997. //=============================================================================
  998. bool TbMillepede::multiplyAX(const std::vector<std::vector<double> >& a,
  999. const std::vector<double>& x,
  1000. std::vector<double>& y, const unsigned int n,
  1001. const unsigned int m) {
  1002.  
  1003. // Y = A * X, where
  1004. // A = general M-by-N matrix
  1005. // X = N vector
  1006. // Y = M vector
  1007. for (unsigned int i = 0; i < m; ++i) {
  1008. y[i] = 0.0;
  1009. for (unsigned int j = 0; j < n; ++j) {
  1010. y[i] += a[i][j] * x[j];
  1011. }
  1012. }
  1013. return true;
  1014. }
  1015.  
  1016. //=============================================================================
  1017. // Multiply symmetric N-by-N matrix V from the left with general M-by-N
  1018. // matrix A and from the right with the transposed of the same general
  1019. // matrix to form a symmetric M-by-M matrix W.
  1020. //=============================================================================
  1021. bool TbMillepede::multiplyAVAt(const std::vector<std::vector<double> >& v,
  1022. const std::vector<std::vector<double> >& a,
  1023. std::vector<std::vector<double> >& w,
  1024. const unsigned int n, const unsigned int m) {
  1025.  
  1026. // W = A * V * AT, where
  1027. // V = symmetric N-by-N matrix
  1028. // A = general N-by-M matrix
  1029. // W = symmetric M-by-M matrix
  1030. for (unsigned int i = 0; i < m; ++i) {
  1031. for (unsigned int j = 0; j <= i; ++j) {
  1032. // Reset the final matrix.
  1033. w[i][j] = 0.0;
  1034. // Fill the upper left triangle.
  1035. for (unsigned int k = 0; k < n; ++k) {
  1036. for (unsigned int l = 0; l < n; ++l) {
  1037. w[i][j] += a[i][k] * v[k][l] * a[j][l];
  1038. }
  1039. }
  1040. // Fill the rest.
  1041. w[j][i] = w[i][j];
  1042. }
  1043. }
  1044.  
  1045. return true;
  1046. }
  1047.  
  1048. //=============================================================================
  1049. // Print results
  1050. //=============================================================================
  1051. bool TbMillepede::printResults() {
  1052. const std::string line(85, '-');
  1053. info() << line << endmsg;
  1054. info() << " Result of fit for global parameters" << endmsg;
  1055. info() << line << endmsg;
  1056. info() << " I Difference Last step "
  1057. << "Error Pull Global corr." << endmsg;
  1058. info() << line << endmsg;
  1059. for (unsigned int i = 0; i < m_nagb; ++i) {
  1060. double err = sqrt(fabs(m_cgmat[i][i]));
  1061. if (m_cgmat[i][i] < 0.0) err = -err;
  1062. if (fabs(m_cgmat[i][i] * m_diag[i]) > 0) {
  1063. info() << std::setprecision(6) << std::fixed;
  1064. // Calculate the pull.
  1065. const double pull =
  1066. m_dparm[i] / sqrt(m_psigm[i] * m_psigm[i] - m_cgmat[i][i]);
  1067. // Calculate the global correlation coefficient
  1068. // (correlation between the parameter and all the other variables).
  1069. const double gcor = sqrt(fabs(1.0 - 1.0 / (m_cgmat[i][i] * m_diag[i])));
  1070. info() << std::setw(4) << i << " " << std::setw(10) << m_dparm[i]
  1071. << " " << std::setw(10) << m_bgvec[i] << " " << std::setw(10)
  1072. << std::setprecision(5) << err << " " << std::setw(10)
  1073. << std::setprecision(5) << pull << " " << std::setw(10) << gcor
  1074. << endmsg;
  1075. } else {
  1076. info() << std::setw(4) << i << " " << std::setw(10) << "OFF"
  1077. << " " << std::setw(10) << "OFF"
  1078. << " " << std::setw(10) << "OFF"
  1079. << " " << std::setw(10) << "OFF"
  1080. << " " << std::setw(10) << "OFF" << endmsg;
  1081. }
  1082. if ((i + 1) % (m_nagb / 6) == 0) info() << line << endmsg;
  1083. }
  1084. if (m_debug) {
  1085. for (unsigned int i = 0; i < m_nagb; ++i) {
  1086. debug() << " i=" << i
  1087. << " sqrt(fabs(cgmat[i][i]))=" << sqrt(fabs(m_cgmat[i][i]))
  1088. << " diag = " << m_diag[i] << endmsg;
  1089. }
  1090. }
  1091.  
  1092. return true;
  1093. }