#include <iomanip>
#include <algorithm>
#include <cmath>
// Local
#include "TbMillepede.h"
// TbKernel
#include "TbKernel/TbGeomFunctions.h"
DECLARE_TOOL_FACTORY(TbMillepede)
//=============================================================================
// Standard constructor, initializes variables
//=============================================================================
TbMillepede::TbMillepede(const std::string& type, const std::string& name,
const IInterface* parent)
: TbAlignmentBase(type, name, parent),
m_nalc(4),
m_debug(false),
m_iterate(true) {
declareProperty("NIterations", m_nIterations = 5);
declareProperty("ResCut", m_rescut = 0.05);
declareProperty("ResCutInit", m_rescut_init = 0.6);
declareProperty("NStdDev", m_nstdev = 0);
declareProperty("Sigmas", m_sigmas = {});
m_dofsDefault = {true, true, false, true, true, true};
}
//=============================================================================
// Destructor
//=============================================================================
TbMillepede::~TbMillepede() {}
//=============================================================================
// Initialization
//=============================================================================
StatusCode TbMillepede::initialize() {
// Initialise the base class.
StatusCode sc = TbAlignmentBase::initialize();
if (sc.isFailure()) return sc;
// Get the verbosity level.
m_debug = msgLevel(MSG::DEBUG);
// Renumber the planes in Millepede, ignoring masked planes.
m_millePlanes.assign(m_nPlanes, 999);
unsigned int index = 0;
for (unsigned int i = 0; i < m_nPlanes; ++i) {
if (masked(i)) continue;
m_millePlanes[i] = index;
++index;
}
// Use default values for the sigmas, unless specified explicitly.
if (m_sigmas.size() != 6) {
m_sigmas = {0.05, 0.05, 0.5, 0.005, 0.005, 0.005};
}
return StatusCode::SUCCESS;
}
//=============================================================================
// Main alignment function
//=============================================================================
void TbMillepede::align(std::vector<TbAlignmentTrack*>& alignmentTracks) {
info() << "Millepede alignment" << endmsg;
const unsigned int nPlanes = m_nPlanes - m_maskedPlanes.size();
const unsigned int nParameters = 6 * nPlanes;
for (unsigned int iteration = 0; iteration < m_nIterations; ++iteration) {
const unsigned int nTracks = alignmentTracks.size();
// Define the constraint equations.
setConstraints(nPlanes);
const double startfact = 100.;
// Initialise all matrices and vectors.
reset(nPlanes, startfact);
info() << "Feeding Millepede with " << nTracks << " tracks..." << endmsg;
// Feed Millepede with tracks.
unsigned int nSkipped = 0;
unsigned int nOutliers = 0;
for (unsigned int i = 0; i < nTracks; ++i) {
LHCb::TbTrack* track = alignmentTracks[i]->track();
if (track->size() != nPlanes) {
++nSkipped;
continue;
}
if (!putTrack(track, nPlanes)) {
++nOutliers;
}
}
if (nSkipped > 0) {
info() << "Skipped " << nSkipped << " tracks with less than " << nPlanes
<< " clusters." << endmsg;
}
if (nOutliers > 0) {
info() << "Rejected " << nOutliers << " outlier tracks." << endmsg;
}
// Do the global fit.
info() << "Determining global parameters..." << endmsg;
fitGlobal();
// Calculate the convergence metric (sum of misalignments).
double converg = 0.;
for (unsigned int i = 0; i < nParameters; ++i) {
converg += fabs(m_dparm[i]);
}
converg /= nParameters;
info() << "Convergence: " << converg << endmsg;
// Update the module positions and orientations.
info() << "Updating geometry..." << endmsg;
updateGeometry();
// Update the cluster coordinates based on the new geometry.
for (unsigned int i = 0; i < nTracks; ++i) {
LHCb::TbTrack* track = alignmentTracks[i]->track();
SmartRefVector<LHCb::TbCluster> clusters = track->clusters();
for (auto ic = clusters.begin(), end = clusters.end(); ic != end; ++ic) {
Gaudi::XYZPoint pLocal((*ic)->xloc(), (*ic)->yloc(), 0.);
const auto pGlobal = geomSvc()->localToGlobal(pLocal, (*ic)->plane());
(*ic)->setX(pGlobal.x());
(*ic)->setY(pGlobal.y());
(*ic)->setZ(pGlobal.z());
}
}
if (converg < 0.00001) break;
}
}
//=============================================================================
// Setup the constraint equations.
//=============================================================================
void TbMillepede::setConstraints(const unsigned int nPlanes) {
// Calculate the mean z-position.
double avgz = 0.;
for (auto im = m_modules.cbegin(), end = m_modules.cend(); im != end; ++im) {
if (masked(im - m_modules.cbegin())) continue;
avgz += (*im)->z();
}
avgz /= nPlanes;
// Calculate the variance.
double varz = 0.0;
for (auto im = m_modules.cbegin(), end = m_modules.cend(); im != end; ++im) {
if (masked(im - m_modules.cbegin())) continue;
const double dz = (*im)->z() - avgz;
varz += dz * dz;
}
varz /= nPlanes;
// Define the 9 constraints equations according to the requested geometry.
const unsigned int nParameters = 6 * nPlanes;
std::vector<double> ftx(nParameters, 0.);
std::vector<double> fty(nParameters, 0.);
std::vector<double> ftz(nParameters, 0.);
std::vector<double> frx(nParameters, 0.);
std::vector<double> fry(nParameters, 0.);
std::vector<double> frz(nParameters, 0.);
std::vector<double> fscaz(nParameters, 0.);
std::vector<double> shearx(nParameters, 0.);
std::vector<double> sheary(nParameters, 0.);
m_constraints.clear();
for (auto im = m_modules.cbegin(), end = m_modules.cend(); im != end; ++im) {
if (masked(im - m_modules.begin())) continue;
const unsigned int i = m_millePlanes[im - m_modules.begin()];
const double sz = ((*im)->z() - avgz) / varz;
ftx[i] = 1.0;
fty[i + nPlanes] = 1.0;
ftz[i + 2 * nPlanes] = 1.0;
frx[i + 3 * nPlanes] = 1.0 ; // i > 4 ? 1.0 : -1.0;
fry[i + 4 * nPlanes] = 1.0 ; // i > 4 ? 1.0 : -1.0;
frz[i + 5 * nPlanes] = 1.0;
shearx[i] = sz;
sheary[i + nPlanes] = sz;
fscaz[i + 2 * nPlanes] = sz;
}
const std::vector<bool> constraints = {true, true, true, true, false,
false, true, true, true};
// Put the constraints information in the basket.
if (constraints[0] && m_dofs[0]) addConstraint(ftx, 0.0);
if (constraints[1] && m_dofs[0]) addConstraint(shearx, 0.);
if (constraints[2] && m_dofs[1]) addConstraint(fty, 0.0);
if (constraints[3] && m_dofs[1]) addConstraint(sheary, 0.);
if (constraints[4] && m_dofs[2]) addConstraint(ftz, 0.0);
// if (constraints[5] && m_dofs[2]) addConstraint(fscaz, 0.0);
if (constraints[6] && m_dofs[3]) addConstraint(frx, 0.0);
if (constraints[7] && m_dofs[4]) addConstraint(fry, 0.0);
if (constraints[8] && m_dofs[5]) addConstraint(frz, 0.0);
}
//=============================================================================
// Define a single constraint equation.
//=============================================================================
void TbMillepede::addConstraint(const std::vector<double>& dercs,
const double rhs) {
Constraint constraint;
// Set the right-hand side (Lagrange multiplier value, sum of equation).
constraint.rhs = rhs;
// Set the constraint equation coefficients.
constraint.coefficients = dercs;
m_constraints.push_back(constraint);
}
//=============================================================================
// Add the equations for one track to the matrix
//=============================================================================
bool TbMillepede::putTrack(LHCb::TbTrack* track, const unsigned int nPlanes) {
std::vector<Equation> equations;
const unsigned int nParameters = 6 * nPlanes;
// Global derivatives
std::vector<double> dergb(nParameters, 0.);
// Global derivatives non linearly related to residual
std::vector<double> dernl(nParameters, 0.);
// Local parameter indices associated with non-linear derivatives
std::vector<int> dernli(nParameters, 0);
// Track slopes
std::vector<double> dernls(nParameters, 0.);
/// Refit the track for the reference states.
m_trackFit->fit(track);
auto state = track->firstState();
const double tx = state.tx();
const double ty = state.ty();
// Iterate over each cluster on the track.
const SmartRefVector<LHCb::TbCluster>& clusters = track->clusters();
for (auto itc = clusters.cbegin(), end = clusters.cend(); itc != end; ++itc) {
if (masked((*itc)->plane())) continue;
const auto normal = geomSvc()->module((*itc)->plane())->normal();
double nx = normal.x() / normal.z();
double ny = normal.y() / normal.z();
const double xg = (*itc)->x();
const double yg = (*itc)->y();
const double zg = (*itc)->z();
// Calculate quasi-local coordinates.
const double zl = zg - m_modules[(*itc)->plane()]->z();
const double xl = xg - m_modules[(*itc)->plane()]->x();
const double yl = yg - m_modules[(*itc)->plane()]->y();
std::vector<double> nonlinear = {
nx, ny, 1., -yl + zl * ny, -xl + zl * nx, xl * ny - yl * nx};
const double den = 1. + tx * nx + ty * ny;
for (auto& a : nonlinear) a /= den;
// Get the errors on the measured x and y coordinates.
const double errx = (*itc)->xErr();
const double erry = (*itc)->yErr();
// Get the internal plane index in Millepede.
const unsigned int plane = m_millePlanes[(*itc)->plane()];
// Set the local derivatives for the X equation.
std::vector<double> derlc = {1., zg, 0., 0.};
// Set the global derivatives (see LHCb-2005-101) for the X equation.
std::fill(dergb.begin(), dergb.end(), 0.);
std::fill(dernl.begin(), dernl.end(), 0.);
std::fill(dernli.begin(), dernli.end(), 0);
std::fill(dernls.begin(), dernls.end(), 0.);
if (m_dofs[0]) dergb[plane] = -1.;
if (m_dofs[4]) dergb[4 * nPlanes + plane] = -zl;
if (m_dofs[5]) dergb[5 * nPlanes + plane] = yl;
for (unsigned int i = 0; i < 6; ++i) {
if (!m_dofs[i]) continue;
dergb[i * nPlanes + plane] += tx * nonlinear[i];
dernl[i * nPlanes + plane] = nonlinear[i];
dernli[i * nPlanes + plane] = 1;
dernls[i * nPlanes + plane] = tx;
}
// Store the X equation.
addEquation(equations, derlc, dergb, dernl, dernli, dernls, xg, errx);
// Set the local derivatives for the Y equation.
derlc = {0., 0., 1., zg};
// Set the global derivatives (see LHCb-2005-101) for the Y equation.
std::fill(dergb.begin(), dergb.end(), 0.);
std::fill(dernl.begin(), dernl.end(), 0.);
std::fill(dernli.begin(), dernli.end(), 0);
std::fill(dernls.begin(), dernls.end(), 0.);
if (m_dofs[1]) dergb[nPlanes + plane] = -1.;
if (m_dofs[3]) dergb[3 * nPlanes + plane] = -zl;
if (m_dofs[5]) dergb[5 * nPlanes + plane] = -xl;
for (unsigned int i = 0; i < 6; ++i) {
if (!m_dofs[i]) continue;
dergb[i * nPlanes + plane] += ty * nonlinear[i];
dernl[i * nPlanes + plane] = nonlinear[i];
dernli[i * nPlanes + plane] = 3;
dernls[i * nPlanes + plane] = ty;
}
// Store the Y equation.
addEquation(equations, derlc, dergb, dernl, dernli, dernls, yg, erry);
}
// Vector containing the track parameters
std::vector<double> trackParams(2 * m_nalc + 2, 0.);
// Fit the track.
const unsigned int iteration = 1;
const bool ok = fitTrack(equations, trackParams, false, iteration);
if (ok) m_equations.push_back(equations);
return ok;
}
//=============================================================================
// Store the parameters for one measurement
//=============================================================================
void TbMillepede::addEquation(std::vector<Equation>& equations,
const std::vector<double>& derlc,
const std::vector<double>& dergb,
const std::vector<double>& dernl,
const std::vector<int>& dernli,
const std::vector<double>& slopes,
const double rmeas, const double sigma) {
if (sigma <= 0.) {
error() << "Invalid cluster error (" << sigma << ")" << endmsg;
return;
}
Equation equation;
equation.rmeas = rmeas;
equation.weight = 1. / (sigma * sigma);
// Add non-zero local derivatives and corresponding indices.
equation.derL.clear();
equation.indL.clear();
for (unsigned int i = 0; i < m_nalc; ++i) {
if (derlc[i] == 0.) continue;
equation.indL.push_back(i);
equation.derL.push_back(derlc[i]);
}
// Add non-zero global derivatives and corresponding indices.
equation.derG.clear();
equation.indG.clear();
equation.derNL.clear();
equation.indNL.clear();
equation.slopes.clear();
const unsigned int nG = dergb.size();
for (unsigned int i = 0; i < nG; ++i) {
if (dergb[i] == 0.) continue;
equation.indG.push_back(i);
equation.derG.push_back(dergb[i]);
equation.derNL.push_back(dernl[i]);
equation.indNL.push_back(dernli[i]);
equation.slopes.push_back(slopes[i]);
}
// Add the equation to the list.
equations.push_back(std::move(equation));
}
//=============================================================================
// Track fit (local fit)
//=============================================================================
bool TbMillepede::fitTrack(const std::vector<Equation>& equations,
std::vector<double>& trackParams,
const bool singlefit, const unsigned int iteration) {
std::vector<double> blvec(m_nalc, 0.);
std::vector<std::vector<double> > clmat(m_nalc,
std::vector<double>(m_nalc, 0.));
// First loop: local track fit
for (const auto& equation : equations) {
double rmeas = equation.rmeas;
// Suppress the global part (only relevant with iterations).
const unsigned int nG = equation.derG.size();
for (unsigned int i = 0; i < nG; ++i) {
const unsigned int j = equation.indG[i];
rmeas -= equation.derG[i] * m_dparm[j];
}
const double w = equation.weight;
// Fill local matrix and vector.
const unsigned int nL = equation.derL.size();
for (unsigned int i = 0; i < nL; ++i) {
const unsigned int j = equation.indL[i];
blvec[j] += w * rmeas * equation.derL[i];
if (m_debug) debug() << "blvec[" << j << "] = " << blvec[j] << endmsg;
// Symmetric matrix, don't bother k > j coefficients.
for (unsigned int k = 0; k <= i; ++k) {
const unsigned int ik = equation.indL[k];
clmat[j][ik] += w * equation.derL[i] * equation.derL[k];
if (m_debug) {
debug() << "clmat[" << j << "][" << ik << "] = " << clmat[j][ik]
<< endmsg;
}
}
}
}
// Local parameter matrix is completed, now invert to solve.
// Rank: number of non-zero diagonal elements.
int rank = invertMatrixLocal(clmat, blvec, m_nalc);
// Store the track parameters and errors.
for (unsigned int i = 0; i < m_nalc; ++i) {
trackParams[2 * i] = blvec[i];
trackParams[2 * i + 1] = sqrt(fabs(clmat[i][i]));
}
// Second loop: residual calculation.
double chi2 = 0.0;
int ndf = 0;
for (const auto& equation : equations) {
double rmeas = equation.rmeas;
// Suppress global and local terms.
const unsigned int nL = equation.derL.size();
for (unsigned int i = 0; i < nL; ++i) {
const unsigned int j = equation.indL[i];
rmeas -= equation.derL[i] * blvec[j];
}
const unsigned int nG = equation.derG.size();
for (unsigned int i = 0; i < nG; ++i) {
const unsigned int j = equation.indG[i];
rmeas -= equation.derG[i] * m_dparm[j];
}
// Now rmeas contains the residual value.
if (m_debug) debug() << "Residual value: " << rmeas << endmsg;
// Reject the track if the residual is too large (outlier).
const double rescut = iteration <= 1 ? m_rescut_init : m_rescut;
if (fabs(rmeas) >= rescut) {
if (m_debug) {
debug() << "Reject track due to residual cut in iteration " << iteration
<< endmsg;
}
return false;
}
chi2 += equation.weight * rmeas * rmeas;
++ndf;
}
ndf -= rank;
const bool printDetails = false;
if (printDetails) {
info() << endmsg;
info() << "Local track fit (rank " << rank << ")" << endmsg;
info() << " Result of local fit : (index/parameter/error)" << endmsg;
for (unsigned int i = 0; i < m_nalc; ++i) {
info() << std::setprecision(6) << std::fixed << std::setw(20) << i
<< " / " << std::setw(10) << blvec[i] << " / "
<< sqrt(clmat[i][i]) << endmsg;
}
info() << "Final chi square / degrees of freedom: " << chi2 << " / " << ndf
<< endmsg;
}
// Stop here if just updating the track parameters.
if (singlefit) return true;
if (m_nstdev != 0 && ndf > 0) {
const double chi2PerNdf = chi2 / ndf;
const double cut = chi2Limit(m_nstdev, ndf) * m_cfactr;
if (chi2 > cut) {
// Reject the track.
if (m_debug) {
debug() << "Rejected track because chi2 / ndof (" << chi2PerNdf
<< ") is larger than " << cut << endmsg;
}
return false;
}
}
// Store the chi2 and number of degrees of freedom.
trackParams[2 * m_nalc] = ndf;
trackParams[2 * m_nalc + 1] = chi2;
// Local operations are finished. Track is accepted.
// Third loop: update the global parameters (other matrices).
m_clcmat.assign(m_nagb, std::vector<double>(m_nalc, 0.));
unsigned int nagbn = 0;
std::vector<int> indnz(m_nagb, -1);
std::vector<int> indbk(m_nagb, 0);
for (const auto& equation : equations) {
double rmeas = equation.rmeas;
// Suppress the global part.
const unsigned int nG = equation.derG.size();
for (unsigned int i = 0; i < nG; ++i) {
const unsigned int j = equation.indG[i];
rmeas -= equation.derG[i] * m_dparm[j];
}
const double w = equation.weight;
// First of all, the global/global terms.
for (unsigned int i = 0; i < nG; ++i) {
const unsigned int j = equation.indG[i];
m_bgvec[j] += w * rmeas * equation.derG[i];
if (m_debug) debug() << "bgvec[" << j << "] = " << m_bgvec[j] << endmsg;
for (unsigned int k = 0; k < nG; ++k) {
const unsigned int n = equation.indG[k];
m_cgmat[j][n] += w * equation.derG[i] * equation.derG[k];
if (m_debug) {
debug() << "cgmat[" << j << "][" << n << "] = " << m_cgmat[j][n]
<< endmsg;
}
}
}
// Now we have also rectangular matrices containing global/local terms.
const unsigned int nL = equation.derL.size();
for (unsigned int i = 0; i < nG; ++i) {
const unsigned int j = equation.indG[i];
// Index of index.
int ik = indnz[j];
if (ik == -1) {
// New global variable.
indnz[j] = nagbn;
indbk[nagbn] = j;
ik = nagbn;
++nagbn;
}
// Now fill the rectangular matrix.
for (unsigned int k = 0; k < nL; ++k) {
const unsigned int ij = equation.indL[k];
m_clcmat[ik][ij] += w * equation.derG[i] * equation.derL[k];
if (m_debug)
debug() << "clcmat[" << ik << "][" << ij << "] = " << m_clcmat[ik][ij]
<< endmsg;
}
}
}
// Third loop is finished, now we update the correction matrices.
multiplyAVAt(clmat, m_clcmat, m_corrm, m_nalc, nagbn);
multiplyAX(m_clcmat, blvec, m_corrv, m_nalc, nagbn);
for (unsigned int i = 0; i < nagbn; ++i) {
const unsigned int j = indbk[i];
m_bgvec[j] -= m_corrv[i];
for (unsigned int k = 0; k < nagbn; ++k) {
const unsigned int ik = indbk[k];
m_cgmat[j][ik] -= m_corrm[i][k];
}
}
return true;
}
//=============================================================================
// Update the module positions and orientations.
//=============================================================================
void TbMillepede::updateGeometry() {
const unsigned int nPlanes = m_nPlanes - m_maskedPlanes.size();
for (auto im = m_modules.cbegin(), end = m_modules.cend(); im != end; ++im) {
if (masked(im - m_modules.begin())) continue;
const unsigned int plane = m_millePlanes[im - m_modules.begin()];
const double tx = (*im)->x() + m_dparm[plane + 0 * nPlanes];
const double ty = (*im)->y() + m_dparm[plane + 1 * nPlanes];
const double tz = (*im)->z() + m_dparm[plane + 2 * nPlanes];
double rx = 0.;
double ry = 0.;
double rz = 0.;
Tb::SmallRotation((*im)->rotX(), m_dparm[plane + 3 * nPlanes],
(*im)->rotY(), m_dparm[plane + 4 * nPlanes],
m_dparm[plane + 5 * nPlanes], rx, ry, rz);
(*im)->setAlignment(tx, ty, tz, (*im)->rotX() - rx, (*im)->rotY() + ry,
(*im)->rotZ() - rz, 0., 0., 0., 0., 0., 0.);
}
}
//=============================================================================
// Initialise the vectors and arrays.
//=============================================================================
bool TbMillepede::reset(const unsigned int nPlanes, const double startfact) {
info() << " " << endmsg;
info() << " * o o o " << endmsg;
info() << " o o o " << endmsg;
info() << " o ooooo o o o oo ooo oo ooo oo " << endmsg;
info() << " o o o o o o o o o o o o o o o o " << endmsg;
info() << " o o o o o o oooo o o oooo o o oooo " << endmsg;
info() << " o o o o o o o ooo o o o o " << endmsg;
info() << " o o o o o o oo o oo ooo oo ++ starts" << endmsg;
info() << " " << endmsg;
// Reset the list of track equations.
m_equations.clear();
// Set the number of global derivatives.
m_nagb = 6 * nPlanes;
// Reset matrices and vectors.
const unsigned int nRows = m_nagb + m_constraints.size();
m_bgvec.resize(nRows, 0.);
m_cgmat.assign(nRows, std::vector<double>(nRows, 0.));
m_corrv.assign(m_nagb, 0.);
m_corrm.assign(m_nagb, std::vector<double>(m_nagb, 0.));
m_dparm.assign(m_nagb, 0.);
// Define the sigmas for each parameter.
m_fixed.assign(m_nagb, true);
m_psigm.assign(m_nagb, 0.);
for (unsigned int i = 0; i < 6; ++i) {
if (!m_dofs[i]) continue;
for (unsigned int j = i * nPlanes; j < (i + 1) * nPlanes; ++j) {
m_fixed[j] = false;
m_psigm[j] = m_sigmas[i];
}
}
// Fix modules if requested.
for (auto it = m_fixedPlanes.cbegin(); it != m_fixedPlanes.cend(); ++it) {
info() << "You are fixing module " << (*it) << endmsg;
// TODO: check if this is the "millePlanes" index or the regular one.
const unsigned ndofs = m_fix_all ? 6 : 3;
for (unsigned int i = 0; i < ndofs; ++i) {
m_fixed[(*it) + i * nPlanes] = true;
m_psigm[(*it) + i * nPlanes] = 0.;
}
}
// Set the chi2 / ndof cut used in the local track fit.
// Iterations are stopped when the cut factor reaches the ref. value (1).
m_cfactref = 1.;
m_cfactr = std::max(1., startfact);
return true;
}
//=============================================================================
//
//=============================================================================
bool TbMillepede::fitGlobal() {
m_diag.assign(m_nagb, 0.);
std::vector<double> bgvecPrev(m_nagb, 0.);
std::vector<double> trackParams(2 * m_nalc + 2, 0.);
const unsigned int nTracks = m_equations.size();
std::vector<std::vector<double> > localParams(
nTracks, std::vector<double>(m_nalc, 0.));
const unsigned int nMaxIterations = 10;
unsigned int iteration = 1;
unsigned int nGoodTracks = nTracks;
while (iteration <= nMaxIterations) {
info() << "Iteration " << iteration << " (using " << nGoodTracks
<< " tracks)" << endmsg;
// Save the diagonal elements.
for (unsigned int i = 0; i < m_nagb; ++i) {
m_diag[i] = m_cgmat[i][i];
}
unsigned int nFixed = 0;
for (unsigned int i = 0; i < m_nagb; ++i) {
if (m_fixed[i]) {
// Fixed global parameter.
++nFixed;
for (unsigned int j = 0; j < m_nagb; ++j) {
// Reset row and column.
m_cgmat[i][j] = 0.0;
m_cgmat[j][i] = 0.0;
}
} else {
m_cgmat[i][i] += 1. / (m_psigm[i] * m_psigm[i]);
}
}
// Add the constraints equations.
unsigned int nRows = m_nagb;
for (const auto& constraint : m_constraints) {
double sum = constraint.rhs;
for (unsigned int j = 0; j < m_nagb; ++j) {
if (m_psigm[j] == 0.) {
m_cgmat[nRows][j] = 0.0;
m_cgmat[j][nRows] = 0.0;
} else {
m_cgmat[nRows][j] = nGoodTracks * constraint.coefficients[j];
m_cgmat[j][nRows] = m_cgmat[nRows][j];
}
sum -= constraint.coefficients[j] * m_dparm[j];
}
m_cgmat[nRows][nRows] = 0.0;
m_bgvec[nRows] = nGoodTracks * sum;
++nRows;
}
double cor = 0.0;
if (iteration > 1) {
for (unsigned int j = 0; j < m_nagb; ++j) {
for (unsigned int i = 0; i < m_nagb; ++i) {
if (m_fixed[i]) continue;
cor += bgvecPrev[j] * m_cgmat[j][i] * bgvecPrev[i];
if (i == j) {
cor -= bgvecPrev[i] * bgvecPrev[i] / (m_psigm[i] * m_psigm[i]);
}
}
}
}
if (m_debug) debug() << " Final corr. is " << cor << endmsg;
// Do the matrix inversion.
const int rank = invertMatrix(m_cgmat, m_bgvec, nRows);
// Update the global parameters values.
for (unsigned int i = 0; i < m_nagb; ++i) {
m_dparm[i] += m_bgvec[i];
bgvecPrev[i] = m_bgvec[i];
if (m_debug) {
debug() << "bgvec[" << i << "] = " << m_bgvec[i] << endmsg;
debug() << "dparm[" << i << "] = " << m_dparm[i] << endmsg;
debug() << "cgmat[" << i << "][" << i << "] = " << m_cgmat[i][i]
<< endmsg;
debug() << "err = " << sqrt(fabs(m_cgmat[i][i])) << endmsg;
debug() << "cgmat * diag = " << std::setprecision(5)
<< m_cgmat[i][i] * m_diag[i] << endmsg;
}
}
if (nRows - nFixed - rank != 0) {
warning() << "The rank defect of the symmetric " << nRows << " by "
<< nRows << " matrix is " << nRows - nFixed - rank << endmsg;
}
if (!m_iterate) break;
++iteration;
if (iteration == nMaxIterations) break;
// Update the factor in the track chi2 cut.
const double newcfactr = sqrt(m_cfactr);
if (newcfactr > 1.2 * m_cfactref) {
m_cfactr = newcfactr;
} else {
m_cfactr = m_cfactref;
}
if (m_debug) {
debug() << "Refitting tracks with cut factor " << m_cfactr << endmsg;
}
// Reset global variables.
for (unsigned int i = 0; i < nRows; ++i) {
m_bgvec[i] = 0.0;
for (unsigned int j = 0; j < nRows; ++j) m_cgmat[i][j] = 0.0;
}
// Refit the tracks.
double chi2 = 0.;
double ndof = 0.;
nGoodTracks = 0;
for (unsigned int i = 0; i < nTracks; ++i) {
// Skip invalidated tracks.
if (m_equations[i].empty()) continue;
std::vector<Equation> equations(m_equations[i].begin(),
m_equations[i].end());
for (auto equation : equations) {
const unsigned int nG = equation.derG.size();
for (unsigned int j = 0; j < nG; ++j) {
const double t = localParams[i][equation.indNL[j]];
if (t == 0) continue;
equation.derG[j] += equation.derNL[j] * (t - equation.slopes[j]);
}
}
std::fill(trackParams.begin(), trackParams.end(), 0.);
// Refit the track.
bool ok = fitTrack(equations, trackParams, false, iteration);
// Cache the track state.
for (unsigned int j = 0; j < m_nalc; ++j) {
localParams[i][j] = trackParams[2 * j];
}
if (ok) {
// Update the total chi2.
chi2 += trackParams[2 * m_nalc + 1];
ndof += trackParams[2 * m_nalc];
++nGoodTracks;
} else {
// Disable the track.
m_equations[i].clear();
}
}
info() << "Chi2 / DOF after re-fit: " << chi2 / (ndof - nRows) << endmsg;
}
// Print the final results.
printResults();
info() << endmsg;
info() << " * o o o " << endmsg;
info() << " o o o " << endmsg;
info() << " o ooooo o o o oo ooo oo ooo oo " << endmsg;
info() << " o o o o o o o o o o o o o o o o " << endmsg;
info() << " o o o o o o oooo o o oooo o o oooo " << endmsg;
info() << " o o o o o o o ooo o o o o " << endmsg;
info() << " o o o o o o oo o oo ooo oo ++ ends." << endmsg;
info() << " o " << endmsg;
info() << endmsg;
return true;
}
//=============================================================================
// Obtain solution of a system of linear equations with symmetric matrix
// and the inverse (using 'singular-value friendly' GAUSS pivot).
// Solve the equation V * X = B.
// V is replaced by its inverse matrix and B by X, the solution vector
//=============================================================================
int TbMillepede::invertMatrix(std::vector<std::vector<double> >& v,
std::vector<double>& b, const int n) {
int rank = 0;
const double eps = 0.0000000000001;
std::vector<double> diag(n, 0.);
std::vector<bool> used_param(n, true);
std::vector<bool> flag(n, true);
for (int i = 0; i < n; i++) {
for (int j = 0; j <= i; j++) {
v[j][i] = v[i][j];
}
}
// Find max. elements of each row and column.
std::vector<double> r(n, 0.);
std::vector<double> c(n, 0.);
for (int i = 0; i < n; i++) {
for (int j = 0; j < n; j++) {
if (fabs(v[i][j]) >= r[i]) r[i] = fabs(v[i][j]);
if (fabs(v[j][i]) >= c[i]) c[i] = fabs(v[j][i]);
}
}
for (int i = 0; i < n; i++) {
if (0.0 != r[i]) r[i] = 1. / r[i];
if (0.0 != c[i]) c[i] = 1. / c[i];
// Check if max elements are within requested precision.
if (eps >= r[i]) r[i] = 0.0;
if (eps >= c[i]) c[i] = 0.0;
}
// Equilibrate the V matrix
for (int i = 0; i < n; i++) {
for (int j = 0; j < n; j++) {
v[i][j] = sqrt(r[i]) * v[i][j] * sqrt(c[j]);
}
}
for (int i = 0; i < n; i++) {
// Save the absolute values of the diagonal elements.
diag[i] = fabs(v[i][i]);
if (r[i] == 0. && c[i] == 0.) {
// This part is empty (non-linear treatment with non constraints)
flag[i] = false;
used_param[i] = false;
}
}
for (int i = 0; i < n; i++) {
double vkk = 0.0;
int k = -1;
// First look for the pivot, i. e. the max unused diagonal element.
for (int j = 0; j < n; j++) {
if (flag[j] && (fabs(v[j][j]) > std::max(fabs(vkk), eps))) {
vkk = v[j][j];
k = j;
}
}
if (k >= 0) {
// Pivot found.
rank++;
// Use this value.
flag[k] = false;
// Replace pivot by its inverse.
vkk = 1.0 / vkk;
v[k][k] = -vkk;
for (int j = 0; j < n; j++) {
for (int jj = 0; jj < n; jj++) {
if (j != k && jj != k && used_param[j] && used_param[jj]) {
// Other elements (do them first as you use old v[k][j])
v[j][jj] = v[j][jj] - vkk * v[j][k] * v[k][jj];
}
}
}
for (int j = 0; j < n; j++) {
if (j != k && used_param[j]) {
v[j][k] = v[j][k] * vkk;
v[k][j] = v[k][j] * vkk;
}
}
} else {
// No more pivot value (clear those elements)
for (int j = 0; j < n; j++) {
if (flag[j]) {
b[j] = 0.0;
for (int k = 0; k < n; k++) {
v[j][k] = 0.0;
v[k][j] = 0.0;
}
}
}
break;
}
}
// Correct matrix V
for (int i = 0; i < n; i++) {
for (int j = 0; j < n; j++) {
v[i][j] = sqrt(c[i]) * v[i][j] * sqrt(r[j]);
}
}
std::vector<double> temp(n, 0.);
for (int j = 0; j < n; j++) {
// Reverse matrix elements
for (int jj = 0; jj < n; jj++) {
v[j][jj] = -v[j][jj];
temp[j] += v[j][jj] * b[jj];
}
}
for (int j = 0; j < n; j++) {
b[j] = temp[j];
}
return rank;
}
//=============================================================================
// Simplified version.
//=============================================================================
int TbMillepede::invertMatrixLocal(std::vector<std::vector<double> >& v,
std::vector<double>& b, const int n) {
int rank = 0;
const double eps = 0.0000000000001;
std::vector<bool> flag(n, true);
std::vector<double> diag(n, 0.);
for (int i = 0; i < n; i++) {
// Save the absolute values of the diagonal elements.
diag[i] = fabs(v[i][i]);
for (int j = 0; j <= i; j++) {
v[j][i] = v[i][j];
}
}
for (int i = 0; i < n; i++) {
double vkk = 0.0;
int k = -1;
// First look for the pivot, i. e. the max. unused diagonal element.
for (int j = 0; j < n; j++) {
if (flag[j] && (fabs(v[j][j]) > std::max(fabs(vkk), eps * diag[j]))) {
vkk = v[j][j];
k = j;
}
}
if (k >= 0) {
// Pivot found
rank++;
flag[k] = false;
// Replace pivot by its inverse.
vkk = 1.0 / vkk;
v[k][k] = -vkk;
for (int j = 0; j < n; j++) {
for (int jj = 0; jj < n; jj++) {
if (j != k && jj != k) {
// Other elements (do them first as you use old v[k][j])
v[j][jj] = v[j][jj] - vkk * v[j][k] * v[k][jj];
}
}
}
for (int j = 0; j < n; j++) {
if (j != k) {
v[j][k] = v[j][k] * vkk;
v[k][j] = v[k][j] * vkk;
}
}
} else {
// No more pivot values (clear those elements).
for (int j = 0; j < n; j++) {
if (flag[j]) {
b[j] = 0.0;
for (k = 0; k < n; k++) v[j][k] = 0.0;
}
}
break;
}
}
std::vector<double> temp(n, 0.);
for (int j = 0; j < n; j++) {
// Reverse matrix elements
for (int jj = 0; jj < n; jj++) {
v[j][jj] = -v[j][jj];
temp[j] += v[j][jj] * b[jj];
}
}
for (int j = 0; j < n; j++) {
b[j] = temp[j];
}
return rank;
}
//=============================================================================
// Return the limit in chi^2 / nd for n sigmas stdev authorized.
// Only n=1, 2, and 3 are expected in input.
//=============================================================================
double TbMillepede::chi2Limit(const int n, const int nd) const {
constexpr double sn[3] = {0.47523, 1.690140, 2.782170};
constexpr double table[3][30] = {
{1.0000, 1.1479, 1.1753, 1.1798, 1.1775, 1.1730, 1.1680, 1.1630,
1.1581, 1.1536, 1.1493, 1.1454, 1.1417, 1.1383, 1.1351, 1.1321,
1.1293, 1.1266, 1.1242, 1.1218, 1.1196, 1.1175, 1.1155, 1.1136,
1.1119, 1.1101, 1.1085, 1.1070, 1.1055, 1.1040},
{4.0000, 3.0900, 2.6750, 2.4290, 2.2628, 2.1415, 2.0481, 1.9736,
1.9124, 1.8610, 1.8171, 1.7791, 1.7457, 1.7161, 1.6897, 1.6658,
1.6442, 1.6246, 1.6065, 1.5899, 1.5745, 1.5603, 1.5470, 1.5346,
1.5230, 1.5120, 1.5017, 1.4920, 1.4829, 1.4742},
{9.0000, 5.9146, 4.7184, 4.0628, 3.6410, 3.3436, 3.1209, 2.9468,
2.8063, 2.6902, 2.5922, 2.5082, 2.4352, 2.3711, 2.3143, 2.2635,
2.2178, 2.1764, 2.1386, 2.1040, 2.0722, 2.0428, 2.0155, 1.9901,
1.9665, 1.9443, 1.9235, 1.9040, 1.8855, 1.8681}};
if (nd < 1) return 0.;
const int m = std::max(1, std::min(n, 3));
if (nd <= 30) return table[m - 1][nd - 1];
return ((sn[m - 1] + sqrt(float(2 * nd - 3))) *
(sn[m - 1] + sqrt(float(2 * nd - 3)))) /
float(2 * nd - 2);
}
//=============================================================================
// Multiply general M-by-N matrix A and N-vector X
//=============================================================================
bool TbMillepede::multiplyAX(const std::vector<std::vector<double> >& a,
const std::vector<double>& x,
std::vector<double>& y, const unsigned int n,
const unsigned int m) {
// Y = A * X, where
// A = general M-by-N matrix
// X = N vector
// Y = M vector
for (unsigned int i = 0; i < m; ++i) {
y[i] = 0.0;
for (unsigned int j = 0; j < n; ++j) {
y[i] += a[i][j] * x[j];
}
}
return true;
}
//=============================================================================
// Multiply symmetric N-by-N matrix V from the left with general M-by-N
// matrix A and from the right with the transposed of the same general
// matrix to form a symmetric M-by-M matrix W.
//=============================================================================
bool TbMillepede::multiplyAVAt(const std::vector<std::vector<double> >& v,
const std::vector<std::vector<double> >& a,
std::vector<std::vector<double> >& w,
const unsigned int n, const unsigned int m) {
// W = A * V * AT, where
// V = symmetric N-by-N matrix
// A = general N-by-M matrix
// W = symmetric M-by-M matrix
for (unsigned int i = 0; i < m; ++i) {
for (unsigned int j = 0; j <= i; ++j) {
// Reset the final matrix.
w[i][j] = 0.0;
// Fill the upper left triangle.
for (unsigned int k = 0; k < n; ++k) {
for (unsigned int l = 0; l < n; ++l) {
w[i][j] += a[i][k] * v[k][l] * a[j][l];
}
}
// Fill the rest.
w[j][i] = w[i][j];
}
}
return true;
}
//=============================================================================
// Print results
//=============================================================================
bool TbMillepede::printResults() {
const std::string line(85, '-');
info() << line << endmsg;
info() << " Result of fit for global parameters" << endmsg;
info() << line << endmsg;
info() << " I Difference Last step "
<< "Error Pull Global corr." << endmsg;
info() << line << endmsg;
for (unsigned int i = 0; i < m_nagb; ++i) {
double err = sqrt(fabs(m_cgmat[i][i]));
if (m_cgmat[i][i] < 0.0) err = -err;
if (fabs(m_cgmat[i][i] * m_diag[i]) > 0) {
info() << std::setprecision(6) << std::fixed;
// Calculate the pull.
const double pull =
m_dparm[i] / sqrt(m_psigm[i] * m_psigm[i] - m_cgmat[i][i]);
// Calculate the global correlation coefficient
// (correlation between the parameter and all the other variables).
const double gcor = sqrt(fabs(1.0 - 1.0 / (m_cgmat[i][i] * m_diag[i])));
info() << std::setw(4) << i << " " << std::setw(10) << m_dparm[i]
<< " " << std::setw(10) << m_bgvec[i] << " " << std::setw(10)
<< std::setprecision(5) << err << " " << std::setw(10)
<< std::setprecision(5) << pull << " " << std::setw(10) << gcor
<< endmsg;
} else {
info() << std::setw(4) << i << " " << std::setw(10) << "OFF"
<< " " << std::setw(10) << "OFF"
<< " " << std::setw(10) << "OFF"
<< " " << std::setw(10) << "OFF"
<< " " << std::setw(10) << "OFF" << endmsg;
}
if ((i + 1) % (m_nagb / 6) == 0) info() << line << endmsg;
}
if (m_debug) {
for (unsigned int i = 0; i < m_nagb; ++i) {
debug() << " i=" << i
<< " sqrt(fabs(cgmat[i][i]))=" << sqrt(fabs(m_cgmat[i][i]))
<< " diag = " << m_diag[i] << endmsg;
}
}
return true;
}
iaro