BmnAlignHierarchy.h

Go to the documentation of this file.

/*
    BM@N alignment routine
    BM@N experiment at NICA complex, JINR, 2025
 
    Department: Math & Soft Group of HEP lab
    Author:     Claude (Opus 5)
    Supervisor: Igor Polev, polev@jinr.int
 
    BmnAlignHierarchy class header-only implementation
    Hierarchical (multi-level) parametrization of the alignable detector
*/
 
#ifndef BMNALIGNHIERARCHY_H
#define BMNALIGNHIERARCHY_H
 
#include "BmnAlignDefines.h"
#include "BmnDetectorModel.h"
#include "BmnParamLayout.h"
#include "fairlogger/Logger.h"
 
#include <TMatrixD.h>
#include <TString.h>
#include <TVectorD.h>
#include <string>
#include <vector>
 
/*
 *  Hierarchical parametrization
 *  ============================
 *
 *  The flat parametrization assigns 6 rigid-body corrections to every alignable
 *  detector element (leaf). The hierarchical one adds *composite* nodes -
 *  subsystems, stations, half-stations - each carrying its own 6
 *  corrections, so that the effective correction of a leaf becomes, to first order
 *
 *      a_leaf = a_leaf_own + SUM over ancestors J(leaf, ancestor) * a_ancestor
 *
 *  The point of the exercise is presigmas: tight priors on leaf parameters
 *  together with loose priors on composite ones route chi2-ambiguous content
 *  into plausible large-scale motion of whole structures instead of a swarm of
 *  independent small element wiggles.
 *
 *  Propagation Jacobian
 *  --------------------
 *  BmnMeasureModel parametrizes an element as V' = R (V - P) + P + d with the
 *  pivot P on the beam axis at the element's own z. Requiring a composite with
 *  pivot P_c to move its leaves rigidly and matching the two parametrizations
 *  gives,
 *
 *      R_leaf = R_comp
 *      d_leaf = d_comp + (R_comp - I) * dP,   dP = P_leaf - P_comp
 *
 *  which, spelled out, is the 6x6 block stored in J:
 *
 *      dX_leaf = dX_c + theta_c * dPz - psi_c   * dPy
 *      dY_leaf = dY_c - phi_c   * dPz + psi_c   * dPx
 *      dZ_leaf = dZ_c + phi_c   * dPy - theta_c * dPx
 *
 *  With the natural pivot choice on the beam axis (X_c = Y_c = 0) this reduces to
 *
 *      dX_leaf = dX_c + theta_c * dPz
 *      dY_leaf = dY_c - phi_c   * dPz
 *      dZ_leaf = dZ_c
 *
 *  exactly the shear / telescope-rotation weak modes, now carrying an explicit name
 *  and an explicit prior.
 *
 *  Parameter layout
 *  ----------------
 *  The *full* layout: the parameters of every leaf, followed by those of every
 *  composite. The linear system is assembled and solved in it, and the
 *  constraints and presigmas config files address it.
 *
 *  Keeping leaves first means the hierarchical basis extends the leaf basis
 *  rather than replacing it: T = [ I | J ], so with no composite nodes the
 *  hierarchical system is *identical* to the flat one.
 *
 *  Note on exactness
 *  -----------------
 *  PromoteSystem() applies the basis change to the already reduced system produced
 *  by the Millepede dimension reduction. That is algebraically exact as long as J
 *  does not depend on the individual hit, which is why leaf pivots use a fixed
 *  nominal z per element rather than the per-hit z. For planar elements the
 *  residual effect is of order (hit z spread) x (rotation) and negligible;
 *  BmnAligner logs the observed z spread so the assumption stays checkable.
 */
 
class BmnAlignHierarchy
{
  public:
    struct Node_t
    {
        std::string name;          // human readable label used in reports
        Int_t level{0};            // 0 for a root node, +1 per generation down
        Int_t parent{-1};          // index in Nodes(), -1 for a root node
        Int_t leaf{-1};            // detector element index for leaves, -1 for composites
        Int_t composite{-1};       // order among composites for composites, -1 for leaves
        SVect3 pivot{};            // rotation centre in the global frame
        Bool_t pivotSet{kFALSE};   // kFALSE means "average the owned leaves"
    };
 
    BmnAlignHierarchy() = default;
    virtual ~BmnAlignHierarchy() = default;
 
    // --- construction: BeginBuild, then any number of AddComposite, then EndBuild ---
    // leafZ holds the nominal z of every element (see the note on exactness above)
    inline void BeginBuild(const BmnDetectorModel& detModel, const VecDouble_t& leafZ);
    inline Bool_t AddComposite(const std::string& name,
                               const std::string& parent,   // empty for a root node
                               const VecInt_t& leaves,      // detector element indices owned directly
                               const SVect3* pivot          // null to average the owned leaves
    );
    inline Bool_t EndBuild();
    inline Bool_t BuildFlat(const BmnDetectorModel& detModel)
    {   // for special cases like BmnAlignerApply
        BeginBuild(detModel, VecDouble_t(detModel.Alignables(), 0.0));
        return EndBuild();
    }
 
    Bool_t Ready() const noexcept { return fReady; }
    const BmnDetectorModel* DetModel() const noexcept { return fpDetModel; }
    const BmnParamLayout& Layout() const noexcept { return fLayout; }
 
    // dimensions of the full layout: <X>Count counts objects, <X>Params their 6 corrections each
    Int_t LeafCount() const noexcept { return fLeavesTotal; }
    Int_t CompositeCount() const noexcept { return fCompositesTotal; }
    Int_t LeafParams() const noexcept { return fLeavesTotal * BMN_GLOBAL_PARAMS_PD; }
    Int_t CompositeParams() const noexcept { return fCompositesTotal * BMN_GLOBAL_PARAMS_PD; }
    Int_t AllParams() const noexcept { return LeafParams() + CompositeParams(); }
    Int_t CompositeParamBase() const noexcept { return LeafParams(); }
 
    /*
     *  Transformation scheme. Hierarchical <==> effective values.
     */
 
    const std::vector<Node_t>& Nodes() const noexcept { return fNodes; }
    const TMatrixD& J() const noexcept { return fJ; }
 
    inline void ToLeaf(const TVectorD& hier, TVectorD& leaf) const;
    inline void ToEffectiveCov(const TMatrixD& hier, TMatrixD& eff) const;
    inline TMatrixD PromoteConstraints(const TMatrixD& constraints) const;
    inline void PromoteSystem(const TVectorD& vB, const TMatrixD& mC, TVectorD& vB_h, TMatrixD& mC_h) const;
 
  protected:
    const BmnDetectorModel* fpDetModel{nullptr};   // set by BeginBuild()
    Bool_t fReady{kFALSE};
    Int_t fCompositesTotal{0}, fLeavesTotal{0};
    BmnParamLayout fLayout;       // derived view, built by EndBuild()
    std::vector<Node_t> fNodes;   // leaves first, composite nodes after them
    VecInt_t fCompositeNode;      // composite order -> node index
    TMatrixD fJ;                  // LeafParams() x CompositeParams()
 
    inline Int_t FindNode(const std::string& name) const;
    inline void FillBlock(Int_t row, Int_t col, const SVect3& dP);
};
 
// === inline implementation ====================================================
 
inline void BmnAlignHierarchy::BeginBuild(const BmnDetectorModel& detModel, const VecDouble_t& leafZ)
{
    fReady = kFALSE;
    fpDetModel = &detModel;
    fCompositesTotal = 0;
    fLeavesTotal = detModel.Alignables();
    fNodes.clear();
    fCompositeNode.clear();
 
    // ---- leaf level: one node per alignable detector element ----
    fNodes.reserve(fLeavesTotal);
    for (Int_t i = 0; i < fLeavesTotal; i++) {
        Node_t node;
        node.name =
            Form("det %2d (id %9d, %s)", i, detModel.EncodedID(i), detModel.DecodeID(detModel.EncodedID(i)).c_str());
        node.leaf = i;
        // the measurement model rotates an element about the beam axis at its own z
        node.pivot = SVect3(0.0, 0.0, leafZ[i]);
        node.pivotSet = kTRUE;
        fNodes.push_back(node);
    }
}
 
inline Int_t BmnAlignHierarchy::FindNode(const std::string& name) const
{
    for (std::size_t i = 0; i < fNodes.size(); i++)
        if (fNodes[i].name == name)
            return static_cast<Int_t>(i);
    return -1;
}
 
inline Bool_t BmnAlignHierarchy::AddComposite(const std::string& name,
                                              const std::string& parent,
                                              const VecInt_t& leaves,
                                              const SVect3* pivot)
{
    if (fNodes.empty()) {
        LOG(error) << " <!> AddComposite: BeginBuild() was not called";
        return kFALSE;
    }
    if (name.empty()) {
        LOG(error) << " <!> AddComposite: node name must not be empty";
        return kFALSE;
    }
    if (FindNode(name) >= 0) {
        LOG(error) << " <!> AddComposite: duplicate node name '" << name << "'";
        return kFALSE;
    }
    Int_t parentIdx{-1};
    if (!parent.empty()) {
        parentIdx = FindNode(parent);
        if (parentIdx < 0) {
            LOG(error) << " <!> AddComposite: node '" << name << "' refers to unknown parent '" << parent << "'";
            return kFALSE;
        }
        if (fNodes[parentIdx].leaf >= 0) {
            LOG(error) << " <!> AddComposite: node '" << name << "' cannot descend from leaf '" << parent << "'";
            return kFALSE;
        }
    }
 
    Node_t node;
    node.name = name;
    node.parent = parentIdx;
    node.composite = fCompositesTotal;
    if (pivot) {
        node.pivot = *pivot;
        node.pivotSet = kTRUE;
    }
    const Int_t nodeIdx{static_cast<Int_t>(fNodes.size())};
 
    // adopt the listed leaves - each may have one direct parent only
    for (Int_t leafIdx : leaves) {
        if (leafIdx < 0 || leafIdx >= fLeavesTotal) {
            LOG(error) << " <!> AddComposite: node '" << name << "' lists element " << leafIdx << " out of range [0, "
                       << fLeavesTotal << ")";
            return kFALSE;
        }
        if (fNodes[leafIdx].parent >= 0) {
            LOG(error) << " <!> AddComposite: element " << leafIdx << " already belongs to '"
                       << fNodes[fNodes[leafIdx].parent].name << "', cannot also belong to '" << name << "'";
            return kFALSE;
        }
        fNodes[leafIdx].parent = nodeIdx;
    }
 
    fNodes.push_back(node);
    fCompositeNode.push_back(nodeIdx);
    fCompositesTotal++;
    return kTRUE;
}
 
inline Bool_t BmnAlignHierarchy::EndBuild()
{
    if (fNodes.empty()) {
        LOG(error) << " <!> EndBuild: BeginBuild() was not called";
        return kFALSE;
    }
 
    // ---- levels, and cycle detection along the way ----
    for (Node_t& node : fNodes) {
        Int_t level{0}, walk{node.parent};
        while (walk >= 0) {
            if (++level > static_cast<Int_t>(fNodes.size())) {
                LOG(error) << " <!> EndBuild: parent cycle detected at node '" << node.name << "'";
                return kFALSE;
            }
            walk = fNodes[walk].parent;
        }
        node.level = level;
    }
 
    // ---- default pivots: centre of gravity of the owned leaves ----
    for (Int_t nodeIdx : fCompositeNode) {
        Node_t& node{fNodes[nodeIdx]};
        if (node.pivotSet)
            continue;
        SVect3 sum;
        Int_t cnt{0};
        for (Int_t l = 0; l < fLeavesTotal; l++) {
            for (Int_t walk = fNodes[l].parent; walk >= 0; walk = fNodes[walk].parent)
                if (walk == nodeIdx) {
                    sum += fNodes[l].pivot;
                    cnt++;
                    break;
                }
        }
        if (!cnt) {
            LOG(error) << " <!> EndBuild: composite '" << node.name << "' owns no detector element";
            return kFALSE;
        }
        node.pivot = sum / static_cast<Double_t>(cnt);
        node.pivotSet = kTRUE;
        LOG_IF(warning, cnt == 1) << "  ! composite '" << node.name
                                  << "' owns a single element: its motion is"
                                     " already fully described by that element's own parameters";
    }
 
    /*
     *  Every composite makes T = [ I | J ] rank deficient by its own 6 columns -
     *  that is the intended freedom. A composite owning leaves only through
     *  child nodes is different in kind: its parameters lie in the span of the
     *  children's, so J itself loses 6 columns. What that costs is the ability
     *  to pin the node with leaf priors, because the corresponding null vector
     *  of T is (-J c, c) with J c = 0, that is with a *zero leaf part* - no
     *  presigma placed on a leaf can see it. Such a node needs presigmas of its
     *  own, or the parent-versus-children split stays undetermined.
     */
    for (Int_t nodeIdx : fCompositeNode) {
        Bool_t ownsDirect{kFALSE};
        for (Int_t l = 0; l < fLeavesTotal && !ownsDirect; l++)
            ownsDirect = fNodes[l].parent == nodeIdx;
        LOG_IF(warning, !ownsDirect) << "  ! composite '" << fNodes[nodeIdx].name
                                     << "' owns every element"
                                        " through child nodes: its parameters are a combination of theirs,"
                                        " so the data cannot separate the two levels at all - the split"
                                        " rests on the presigmas alone and converges slowly";
    }
 
    // ---- propagation Jacobian ----
    if (fCompositesTotal) {
        fJ.ResizeTo(LeafParams(), CompositeParams());
        fJ.Zero();
        for (Int_t l = 0; l < fLeavesTotal; l++)
            for (Int_t walk = fNodes[l].parent; walk >= 0; walk = fNodes[walk].parent)
                FillBlock(l * BMN_GLOBAL_PARAMS_PD, fNodes[walk].composite * BMN_GLOBAL_PARAMS_PD,
                          fNodes[l].pivot - fNodes[walk].pivot);
    } else
        fJ.ResizeTo(0, 0);
 
    fReady = kTRUE;
    fLayout.Rebuild(*this);
    LOG(info) << "    " << fNodes.size() << " hierarchy node(s): " << fLeavesTotal << " leaves, " << fCompositesTotal
              << " composite(s)";
    for (Int_t c = 0; c < fCompositesTotal; c++) {
        const Node_t& node{fNodes[fCompositeNode[c]]};
        LOG(info) << Form("      level %d : %-12s pivot (%.3f, %.3f, %.3f) cm", node.level, node.name.c_str(),
                          node.pivot(iX), node.pivot(iY), node.pivot(iZ));
    }
    return fReady;
}
 
inline void BmnAlignHierarchy::FillBlock(Int_t row, Int_t col, const SVect3& dP)
{
    // 6x6 propagation block, see the derivation in the header comment above
    for (Int_t p = 0; p < BMN_GLOBAL_PARAMS_PD; p++)
        fJ(row + p, col + p) = 1.0;   // translations and angles carry over
    fJ(row + idX, col + iTheta) = dP(iZ);
    fJ(row + idX, col + iPsi) = -dP(iY);
    fJ(row + idY, col + iPhi) = -dP(iZ);
    fJ(row + idY, col + iPsi) = dP(iX);
    fJ(row + idZ, col + iPhi) = dP(iY);
    fJ(row + idZ, col + iTheta) = -dP(iX);
}
 
/*
 *  A constraint is written in the full layout, as coefficients on leaf and
 *  composite parameters, one row per constraint. Its row in the solved system is
 *
 *      [ c_leaf ; J^T c_leaf + c_composite ]
 *
 *  the J^T term being what keeps a coefficient placed on a leaf constraining
 *  the same physical quantity once composites carry part of the correction.
 *  For all rows at once that is [ C_leaf | C_leaf J + C_composite ].
 */
inline TMatrixD BmnAlignHierarchy::PromoteConstraints(const TMatrixD& constraints) const
{
    if (!fCompositesTotal || !constraints.GetNrows())
        return constraints;
 
    const Int_t nLeaf{LeafParams()}, lastRow{constraints.GetNrows() - 1};
    TMatrixD cComp{constraints.GetSub(0, lastRow, nLeaf, AllParams() - 1)};
    cComp += TMatrixD(constraints.GetSub(0, lastRow, 0, nLeaf - 1), TMatrixD::kMult, fJ);
 
    TMatrixD rows{constraints};
    rows.SetSub(0, nLeaf, cComp);
    return rows;
}
 
inline void BmnAlignHierarchy::PromoteSystem(const TVectorD& vB,
                                             const TMatrixD& mC,
                                             TVectorD& vB_h,
                                             TMatrixD& mC_h) const
{
    const Int_t nLeaf{LeafParams()}, nHier{AllParams()};
    vB_h.ResizeTo(nHier);
    mC_h.ResizeTo(nHier, nHier);
    vB_h.Zero();
    mC_h.Zero();
    // leading block: the leaf-basis system enters unchanged, since T = [ I | J ]
    vB_h.SetSub(0, vB);
    mC_h.SetSub(0, 0, mC);
    if (!fCompositesTotal)
        return;
 
    // C' = T^T C T = [ C , C J ; (C J)^T , J^T C J ],  b' = T^T b = [ b ; J^T b ]
    TMatrixD mCJ{mC, TMatrixD::kMult, fJ};     // nLeaf x nComp
    TMatrixD mJT{TMatrixD::kTransposed, fJ};   // nComp x nLeaf
    mC_h.SetSub(0, nLeaf, mCJ);
    mC_h.SetSub(nLeaf, 0, TMatrixD(TMatrixD::kTransposed, mCJ));
    mC_h.SetSub(nLeaf, nLeaf, TMatrixD(mJT, TMatrixD::kMult, mCJ));
    vB_h.SetSub(nLeaf, mJT * vB);
}
 
inline void BmnAlignHierarchy::ToLeaf(const TVectorD& hier, TVectorD& leaf) const
{
    const Int_t nLeaf{LeafParams()};
    leaf.ResizeTo(nLeaf);
    hier.GetSub(0, nLeaf - 1, leaf);
    if (!fCompositesTotal)
        return;
    // a_leaf = a_leaf_own + J * a_composite
    TVectorD comp(CompositeParams());
    hier.GetSub(nLeaf, AllParams() - 1, comp);
    leaf += fJ * comp;
}
 
inline void BmnAlignHierarchy::ToEffectiveCov(const TMatrixD& hier, TMatrixD& eff) const
{
    /*
     *  The results are quoted in a mixed basis: a leaf reports its *effective*
     *  correction a_leaf = a_leaf_own + J a_composite, a composite its own
     *  share. That is the linear map
     *
     *      S = [ I  J ]     applied as   Cov_eff = S Cov S^T
     *          [ 0  I ]
     *
     *  giving, block by block,
     *
     *      ll : Cov_ll + J Cov_cl + Cov_lc J^T + J Cov_cc J^T
     *      lc : Cov_lc + J Cov_cc          cl : Cov_cl + Cov_cc J^T
     *      cc : Cov_cc                     (a composite is solved directly)
     *
     *  The ll leading term alone would be the uncertainty of the element's own
     *  part; the rest is the share it inherits from its ancestors, which is what
     *  makes a tightly-primed leaf inside a loosely-primed composite come out
     *  with an honest error rather than the prior's.
     */
    eff.ResizeTo(hier);
    eff = hier;
    if (!fCompositesTotal)
        return;
 
    const Int_t nLeaf{LeafParams()}, last{AllParams() - 1};
    // taken as separate blocks rather than transposed into one another: a
    // numerically inverted matrix is only symmetric up to rounding
    const TMatrixD covLC{hier.GetSub(0, nLeaf - 1, nLeaf, last)}, covCL{hier.GetSub(nLeaf, last, 0, nLeaf - 1)},
        covCC{hier.GetSub(nLeaf, last, nLeaf, last)};
    TMatrixD ll{hier.GetSub(0, nLeaf - 1, 0, nLeaf - 1)};
    ll += TMatrixD(covLC, TMatrixD::kMultTranspose, fJ);
    ll += TMatrixD(fJ, TMatrixD::kMult, covCL);
    ll += TMatrixD(fJ, TMatrixD::kMult, TMatrixD(covCC, TMatrixD::kMultTranspose, fJ));
    TMatrixD lc{covLC};
    lc += TMatrixD(fJ, TMatrixD::kMult, covCC);
    TMatrixD cl{covCL};
    cl += TMatrixD(covCC, TMatrixD::kMultTranspose, fJ);
 
    eff.SetSub(0, 0, ll);
    eff.SetSub(0, nLeaf, lc);
    eff.SetSub(nLeaf, 0, cl);   // the cc block is already in place
}
 
#endif   // BMNALIGNHIERARCHY_H