Commit 747c83f6 authored by Claus Kleinwort's avatar Claus Kleinwort
Browse files

Composed trajectories: geometric constraint added (to kinematic)

git-svn-id: http://svnsrv.desy.de/public/GeneralBrokenLines/trunk@148 281f6f2b-e318-4fd1-8bce-1a4ba7aab212
parent 135f53a2
......@@ -6,8 +6,8 @@ PROJECT(GBL)
# project version
SET( ${PROJECT_NAME}_VERSION_MAJOR 2 )
SET( ${PROJECT_NAME}_VERSION_MINOR 1 )
SET( ${PROJECT_NAME}_VERSION_PATCH 6 )
SET( ${PROJECT_NAME}_VERSION_MINOR 2 )
SET( ${PROJECT_NAME}_VERSION_PATCH 0 )
# make life easier and simply use the ilcsoft default settings
# load default ilcsoft settings (install prefix, build type, rpath, etc.)
......
......@@ -361,7 +361,7 @@ GblDetectorLayer::GblDetectorLayer(const std::string aName,
GblDetectorLayer::~GblDetectorLayer() {
}
/// Print GblSiliconlayer.
/// Print GblDetectorLayer.
void GblDetectorLayer::print() const {
IOFormat CleanFmt(4, 0, ", ", "\n", "[", "]");
std::cout << " Layer " << name << " " << layer << " : " << measDim << "D, "
......@@ -391,14 +391,27 @@ Eigen::Vector2d GblDetectorLayer::getPrecision() const {
return precision;
}
/// Get center.
Eigen::Vector3d GblDetectorLayer::getCenter() const {
return center;
}
/// Get directions of measurement system.
/**
* Matrix from row vectors (transformation from global to measurement system)
*/
const Eigen::Matrix3d& GblDetectorLayer::getMeasSystemDirs() const {
Eigen::Matrix3d GblDetectorLayer::getMeasSystemDirs() const {
return global2meas;;
}
/// Get directions of alignment system.
/**
* Matrix from row vectors (transformation from global to alignment system)
*/
Eigen::Matrix3d GblDetectorLayer::getAlignSystemDirs() const {
return global2align;;
}
/// Intersect with helix.
/**
* \param [in] hlx helix
......@@ -413,7 +426,7 @@ GblHelixPrediction GblDetectorLayer::intersectWithHelix(
* \param[in] position position (of prediction or measurement)
* \param[in] direction track direction
*/
const Matrix<double, 3, 6> GblDetectorLayer::getRigidBodyDerGlobal(
Matrix<double, 3, 6> GblDetectorLayer::getRigidBodyDerGlobal(
Eigen::Vector3d& position, Eigen::Vector3d& direction) const {
// lever arms (for rotations)
Vector3d dist = position;
......@@ -435,14 +448,20 @@ const Matrix<double, 3, 6> GblDetectorLayer::getRigidBodyDerGlobal(
return global2meas * drdm * dmdg;
}
/// Get rigid body derivatives in local (alignment) frame (with N=(0,0,1)).
/// Get rigid body derivatives in local (alignment) frame (rotated in measurement plane).
/**
* Normal to measurement plane has to be (0,0,1) in local frame.
* The orthogonal alignment frame differs from measurement frame only by rotations
* around normal to measurement plane.
*
* Equivalent to:
* \code
* getRigidBodyDerGlobal(position, direction) * getTrafoLocalToGlobal(center, global2align)
* \endcode
*
* \param[in] position position (of prediction or measurement)
* \param[in] direction track direction
*/
const Matrix<double, 2, 6> GblDetectorLayer::getRigidBodyDerLocal(
Matrix<double, 2, 6> GblDetectorLayer::getRigidBodyDerLocal(
Eigen::Vector3d& position, Eigen::Vector3d& direction) const {
// track direction in local system
Vector3d tLoc = global2align * direction;
......@@ -458,7 +477,47 @@ const Matrix<double, 2, 6> GblDetectorLayer::getRigidBodyDerLocal(
Matrix<double, 2, 6> drldg;
drldg << 1.0, 0.0, -uSlope, vPos * uSlope, -uPos * uSlope, vPos, 0.0, 1.0, -vSlope, vPos
* vSlope, -uPos * vSlope, -uPos;
return drldg;
// local (alignment) to measurement system
Matrix3d local2meas = global2meas * global2align.transpose();
return local2meas.block<2, 2>(0, 0) * drldg;
}
/// Get transformation for rigid body derivatives from global to local (alignment) system.
/**
* local = rotation * (global-offset)
*
* \param[in] offset offset of alignment system
* \param[in] rotation rotation of alignment system
*/
Matrix<double, 6, 6> GblDetectorLayer::getTrafoGlobalToLocal(
Eigen::Vector3d& offset, Eigen::Matrix3d& rotation) const {
// transformation global to local
Matrix<double, 6, 6> glo2loc = Matrix<double, 6, 6>::Zero();
Matrix3d leverArms;
leverArms << 0., offset[2], -offset[1], -offset[2], 0., offset[0], offset[1], -offset[0], 0.;
glo2loc.block<3, 3>(0, 0) = rotation;
glo2loc.block<3, 3>(0, 3) = -rotation * leverArms;
glo2loc.block<3, 3>(3, 3) = rotation;
return glo2loc;
}
/// Get transformation for rigid body derivatives from local (alignment) to global system.
/**
* local = rotation * (global-offset)
*
* \param[in] offset offset of alignment system
* \param[in] rotation rotation of alignment system
*/
Matrix<double, 6, 6> GblDetectorLayer::getTrafoLocalToGlobal(
Eigen::Vector3d& offset, Eigen::Matrix3d& rotation) const {
// transformation local to global
Matrix<double, 6, 6> loc2glo = Matrix<double, 6, 6>::Zero();
Matrix3d leverArms;
leverArms << 0., offset[2], -offset[1], -offset[2], 0., offset[0], offset[1], -offset[0], 0.;
loc2glo.block<3, 3>(0, 0) = rotation.transpose();
loc2glo.block<3, 3>(0, 3) = leverArms * rotation.transpose();
loc2glo.block<3, 3>(3, 3) = rotation.transpose();
return loc2glo;
}
}
......@@ -116,12 +116,18 @@ public:
double getRadiationLength() const;
Eigen::Vector2d getResolution() const;
Eigen::Vector2d getPrecision() const;
const Eigen::Matrix3d& getMeasSystemDirs() const;
Eigen::Vector3d getCenter() const;
Eigen::Matrix3d getMeasSystemDirs() const;
Eigen::Matrix3d getAlignSystemDirs() const;
GblHelixPrediction intersectWithHelix(GblSimpleHelix hlx) const;
const Eigen::Matrix<double, 3, 6> getRigidBodyDerGlobal(
Eigen::Vector3d& position, Eigen::Vector3d& direction) const;
const Eigen::Matrix<double, 2, 6> getRigidBodyDerLocal(
Eigen::Vector3d& position, Eigen::Vector3d& direction) const;
Eigen::Matrix<double, 3, 6> getRigidBodyDerGlobal(Eigen::Vector3d& position,
Eigen::Vector3d& direction) const;
Eigen::Matrix<double, 2, 6> getRigidBodyDerLocal(Eigen::Vector3d& position,
Eigen::Vector3d& direction) const;
Eigen::Matrix<double, 6, 6> getTrafoGlobalToLocal(Eigen::Vector3d& offset,
Eigen::Matrix3d& rotation) const;
Eigen::Matrix<double, 6, 6> getTrafoLocalToGlobal(Eigen::Vector3d& offset,
Eigen::Matrix3d& rotation) const;
private:
std::string name; ///< name
......
......@@ -165,7 +165,8 @@ private:
std::vector<unsigned int> numPoints; ///< Number of points on (sub)trajectory
unsigned int numTrajectories; ///< Number of trajectories (in composed trajectory)
unsigned int numOffsets; ///< Number of (points with) offsets on trajectory
unsigned int numInnerTrans; ///< Number of inner transformations to external parameters
unsigned int numInnerTransformations; ///< Number of inner transformations to external parameters
unsigned int numInnerTransOffsets; ///< Number of (points with) offsets affected by inner transformations to external parameters
unsigned int numCurvature; ///< Number of curvature parameters (0 or 1) or external parameters
unsigned int numParameters; ///< Number of fit parameters
unsigned int numLocals; ///< Total number of (additional) local parameters
......@@ -181,11 +182,13 @@ private:
std::vector<unsigned int> measDataIndex; ///< mapping points to data blocks from measurements
std::vector<unsigned int> scatDataIndex; ///< mapping points to data blocks from scatterers
Eigen::MatrixXd externalSeed; ///< Precision (inverse covariance matrix) of external seed
std::vector<Eigen::MatrixXd> innerTransformations; ///< Transformations at innermost points of
// composed trajectory (from common external parameters)
Eigen::MatrixXd externalDerivatives; // Derivatives for external measurements of composed trajectory
Eigen::VectorXd externalMeasurements; // Residuals for external measurements of composed trajectory
Eigen::VectorXd externalPrecisions; // Precisions for external measurements of composed trajectory
// composed trajectory
std::vector<Eigen::MatrixXd> innerTransformations; ///< Transformations at innermost points of composed trajectory (from common external parameters)
std::vector<Eigen::MatrixXd> innerTransDer; ///< Derivatives at innermost points of composed trajectory
std::vector<std::array<unsigned int, 5> > innerTransLab; ///< Labels at innermost points of composed trajectory
Eigen::MatrixXd externalDerivatives; ///< Derivatives for external measurements of composed trajectory
Eigen::VectorXd externalMeasurements; ///< Residuals for external measurements of composed trajectory
Eigen::VectorXd externalPrecisions; ///< Precisions for external measurements of composed trajectory
// linear equation system
VVector theVector; ///< Vector of linear equation system
BorderedBandMatrix theMatrix; ///< (Bordered band) matrix of linear equation system
......@@ -212,9 +215,9 @@ template<typename Seed>
GblTrajectory::GblTrajectory(const std::vector<GblPoint> &aPointList,
unsigned int aLabel, const Eigen::MatrixBase<Seed>& aSeed,
bool flagCurv, bool flagU1dir, bool flagU2dir) :
numAllPoints(aPointList.size()), numPoints(), numOffsets(0), numInnerTrans(
0), numCurvature(flagCurv ? 1 : 0), numParameters(0), numLocals(
0), numMeasurements(0), externalPoint(aLabel), skippedMeasLabel(
numAllPoints(aPointList.size()), numPoints(), numOffsets(0), numInnerTransformations(
0), numInnerTransOffsets(0), numCurvature(flagCurv ? 1 : 0), numParameters(
0), numLocals(0), numMeasurements(0), externalPoint(aLabel), skippedMeasLabel(
0), maxNumGlobals(0), theDimension(0), thePoints(), theData(), measDataIndex(), scatDataIndex(), externalSeed(
aSeed), innerTransformations(), externalDerivatives(), externalMeasurements(), externalPrecisions() {
......@@ -235,7 +238,7 @@ GblTrajectory::GblTrajectory(
const Eigen::MatrixBase<Derivatives>& extDerivatives,
const Eigen::MatrixBase<Measurements>& extMeasurements,
const Eigen::MatrixBase<Precision>& extPrecisions) :
numAllPoints(), numPoints(), numOffsets(0), numInnerTrans(
numAllPoints(), numPoints(), numOffsets(0), numInnerTransformations(
aPointsAndTransList.size()), numParameters(0), numLocals(0), numMeasurements(
0), externalPoint(0), skippedMeasLabel(0), maxNumGlobals(0), theDimension(
0), thePoints(), theData(), measDataIndex(), scatDataIndex(), externalSeed(), innerTransformations() {
......@@ -264,7 +267,12 @@ GblTrajectory::GblTrajectory(
}
theDimension.push_back(0);
theDimension.push_back(1);
numCurvature = innerTransformations[0].cols();
// kinematic (2) or geometric (1) constraint
numInnerTransOffsets = innerTransformations[0].rows() == 5 ? 2 : 1;
numCurvature =
innerTransformations[0].rows() == 5 ?
innerTransformations[0].cols() :
innerTransformations[0].cols() + numInnerTransformations;
construct(); // construct (composed) trajectory
}
......@@ -275,7 +283,7 @@ GblTrajectory::GblTrajectory(
const Eigen::MatrixBase<Derivatives>& extDerivatives,
const Eigen::MatrixBase<Measurements>& extMeasurements,
const Eigen::MatrixBase<Precision>& extPrecisions) :
numAllPoints(), numPoints(), numOffsets(0), numInnerTrans(
numAllPoints(), numPoints(), numOffsets(0), numInnerTransformations(
aPointsAndTransList.size()), numParameters(0), numLocals(0), numMeasurements(
0), externalPoint(0), skippedMeasLabel(0), maxNumGlobals(0), theDimension(
0), thePoints(), theData(), measDataIndex(), scatDataIndex(), externalSeed(), innerTransformations() {
......@@ -295,7 +303,12 @@ GblTrajectory::GblTrajectory(
}
theDimension.push_back(0);
theDimension.push_back(1);
numCurvature = innerTransformations[0].cols();
// kinematic (2) or geometric (1) constraint
numInnerTransOffsets = innerTransformations[0].rows() == 5 ? 2 : 1;
numCurvature =
innerTransformations[0].rows() == 5 ?
innerTransformations[0].cols() :
innerTransformations[0].cols() + numInnerTransformations;
construct(); // construct (composed) trajectory
}
......
......@@ -72,6 +72,8 @@
* some external parameters (like those describing a decay)
* and transformations at the first points from the external
* to the local track parameters.
* The external parameters can describe the full kinematics (all track parameters)
* or only the geometry (position parameters at common vertex).
*
* The conventions for the coordinate systems follow:
* Derivation of Jacobians for the propagation of covariance
......@@ -160,10 +162,10 @@ namespace gbl {
*/
GblTrajectory::GblTrajectory(const std::vector<GblPoint> &aPointList,
bool flagCurv, bool flagU1dir, bool flagU2dir) :
numAllPoints(aPointList.size()), numPoints(), numOffsets(0), numInnerTrans(
0), numCurvature(flagCurv ? 1 : 0), numParameters(0), numLocals(
0), numMeasurements(0), externalPoint(0), skippedMeasLabel(0), maxNumGlobals(
0), theDimension(0), thePoints(), theData(), measDataIndex(), scatDataIndex(), externalSeed(), innerTransformations(), externalDerivatives(), externalMeasurements(), externalPrecisions() {
numAllPoints(aPointList.size()), numPoints(), numOffsets(0), numInnerTransformations(
0), numInnerTransOffsets(0), numCurvature(flagCurv ? 1 : 0), numParameters(
0), numLocals(0), numMeasurements(0), externalPoint(0), skippedMeasLabel(
0), maxNumGlobals(0), theDimension(0), thePoints(), theData(), measDataIndex(), scatDataIndex(), externalSeed(), innerTransformations(), externalDerivatives(), externalMeasurements(), externalPrecisions() {
if (flagU1dir)
theDimension.push_back(0);
......@@ -182,7 +184,7 @@ GblTrajectory::GblTrajectory(const std::vector<GblPoint> &aPointList,
*/
GblTrajectory::GblTrajectory(
const std::vector<std::pair<std::vector<GblPoint>, Eigen::MatrixXd> >& aPointsAndTransList) :
numAllPoints(), numPoints(), numOffsets(0), numInnerTrans(
numAllPoints(), numPoints(), numOffsets(0), numInnerTransformations(
aPointsAndTransList.size()), numParameters(0), numLocals(0), numMeasurements(
0), externalPoint(0), skippedMeasLabel(0), maxNumGlobals(0), theDimension(
0), thePoints(), theData(), measDataIndex(), scatDataIndex(), externalSeed(), innerTransformations(), externalDerivatives(), externalMeasurements(), externalPrecisions() {
......@@ -195,7 +197,12 @@ GblTrajectory::GblTrajectory(
}
theDimension.push_back(0);
theDimension.push_back(1);
numCurvature = innerTransformations[0].cols();
// kinematic (2) or geometric (1) constraint
numInnerTransOffsets = innerTransformations[0].rows() == 5 ? 2 : 1;
numCurvature =
innerTransformations[0].rows() == 5 ?
innerTransformations[0].cols() :
innerTransformations[0].cols() + numInnerTransformations;
construct(); // construct (composed) trajectory
}
......@@ -216,7 +223,7 @@ GblTrajectory::GblTrajectory(const std::vector<GblPoint> &aPointList,
unsigned int aLabel, const TMatrixDSym &aSeed, bool flagCurv,
bool flagU1dir, bool flagU2dir) :
numAllPoints(aPointList.size()), numPoints(), numOffsets(0),
numInnerTrans(0), numCurvature(flagCurv ? 1 : 0), numParameters(0),
numInnerTransformations(0), numInnerTransOffsets(0), numCurvature(flagCurv ? 1 : 0), numParameters(0),
numLocals(0), numMeasurements(0), externalPoint(aLabel), skippedMeasLabel(0),
maxNumGlobals(0), theDimension(0), thePoints(), theData(), measDataIndex(),
scatDataIndex(), externalSeed(), innerTransformations(), externalDerivatives(),
......@@ -246,7 +253,7 @@ externalMeasurements(), externalPrecisions() {
* \param [in] aPointsAndTransList List containing pairs with list of points and transformation (at inner (first) point)
*/
GblTrajectory::GblTrajectory(const std::vector<std::pair<std::vector<GblPoint>, TMatrixD> > &aPointsAndTransList) :
numAllPoints(), numPoints(), numOffsets(0), numInnerTrans(aPointsAndTransList.size()),
numAllPoints(), numPoints(), numOffsets(0), numInnerTransformations(aPointsAndTransList.size()),
numParameters(0), numLocals(0), numMeasurements(0), externalPoint(0),
skippedMeasLabel(0), maxNumGlobals(0), theDimension(0), thePoints(), theData(),
measDataIndex(), scatDataIndex(), externalSeed(), innerTransformations(),
......@@ -269,8 +276,13 @@ externalDerivatives(), externalMeasurements(), externalPrecisions() {
}
theDimension.push_back(0);
theDimension.push_back(1);
numCurvature = innerTransformations[0].cols();
construct(); // construct (composed) trajectory
// kinematic (2) or geometric (1) constraint
numInnerTransOffsets = innerTransformations[0].rows() == 5 ? 2 : 1;
numCurvature =
innerTransformations[0].rows() == 5 ?
innerTransformations[0].cols() :
innerTransformations[0].cols() + numInnerTransformations;
construct();// construct (composed) trajectory
}
/// Create new composed trajectory from list of points and transformations with (independent) external measurements.
......@@ -284,7 +296,7 @@ externalDerivatives(), externalMeasurements(), externalPrecisions() {
GblTrajectory::GblTrajectory(const std::vector<std::pair<std::vector<GblPoint>, TMatrixD> > &aPointsAndTransList,
const TMatrixD &extDerivatives, const TVectorD &extMeasurements,
const TVectorD &extPrecisions) :
numAllPoints(), numPoints(), numOffsets(0), numInnerTrans(aPointsAndTransList.size()),
numAllPoints(), numPoints(), numOffsets(0), numInnerTransformations(aPointsAndTransList.size()),
numParameters(0), numLocals(0), numMeasurements(0), externalPoint(0),
skippedMeasLabel(0), maxNumGlobals(0), theDimension(0), thePoints(), theData(),
measDataIndex(), scatDataIndex(), externalSeed(), innerTransformations(),
......@@ -320,8 +332,13 @@ externalDerivatives(), externalMeasurements(), externalPrecisions() {
}
theDimension.push_back(0);
theDimension.push_back(1);
numCurvature = innerTransformations[0].cols();
construct(); // construct (composed) trajectory
// kinematic (2) or geometric (1) constraint
numInnerTransOffsets = innerTransformations[0].rows() == 5 ? 2 : 1;
numCurvature =
innerTransformations[0].rows() == 5 ?
innerTransformations[0].cols() :
innerTransformations[0].cols() + numInnerTransformations;
construct();// construct (composed) trajectory
}
/// Create new composed trajectory from list of points and transformations with (correlated) external measurements.
......@@ -335,7 +352,7 @@ externalDerivatives(), externalMeasurements(), externalPrecisions() {
GblTrajectory::GblTrajectory(const std::vector<std::pair<std::vector<GblPoint>, TMatrixD> > &aPointsAndTransList,
const TMatrixD &extDerivatives, const TVectorD &extMeasurements,
const TMatrixDSym &extPrecisions) :
numAllPoints(), numPoints(), numOffsets(0), numInnerTrans(aPointsAndTransList.size()),
numAllPoints(), numPoints(), numOffsets(0), numInnerTransformations(aPointsAndTransList.size()),
numParameters(0), numLocals(0), numMeasurements(0), externalPoint(0),
skippedMeasLabel(0), maxNumGlobals(0), theDimension(0), thePoints(), theData(),
measDataIndex(), scatDataIndex(), externalSeed(), innerTransformations() {
......@@ -377,8 +394,13 @@ measDataIndex(), scatDataIndex(), externalSeed(), innerTransformations() {
}
theDimension.push_back(0);
theDimension.push_back(1);
numCurvature = innerTransformations[0].cols();
construct(); // construct (composed) trajectory
// kinematic (2) or geometric (1) constraint
numInnerTransOffsets = innerTransformations[0].rows() == 5 ? 2 : 1;
numCurvature =
innerTransformations[0].rows() == 5 ?
innerTransformations[0].cols() :
innerTransformations[0].cols() + numInnerTransformations;
construct();// construct (composed) trajectory
}
#endif
......@@ -429,12 +451,17 @@ void GblTrajectory::construct() {
std::cout << " GblTrajectory construction failed: " << e.what()
<< std::endl;
return;
} catch (int i) {
std::cout << " GblTrajectory construction failed with exception " << i
<< std::endl;
return;
}
constructOK = true;
// number of fit parameters
numParameters = (numOffsets - 2 * numInnerTrans) * theDimension.size()
+ numCurvature + numLocals;
numParameters =
(numOffsets - numInnerTransOffsets * numInnerTransformations)
* theDimension.size() + numCurvature + numLocals;
}
/// Define offsets from list of points.
......@@ -504,7 +531,7 @@ void GblTrajectory::calcJacobians() {
/// Get jacobian for transformation from fit to track parameters at point.
/**
* Jacobian broken lines (q/p,..,u_i,u_i+1..) to track (q/p,u',u) parameters
* including additional local parameters.
* including additional local parameters (and transformation from external parameters).
* \param [in] aSignedLabel (Signed) label of point for external seed
* (<0: in front, >0: after point, slope changes at scatterer!)
* \return List of fit parameters with non zero derivatives and
......@@ -516,14 +543,7 @@ std::pair<std::vector<unsigned int>, MatrixXd> GblTrajectory::getJacobian(
unsigned int nDim = theDimension.size();
unsigned int nCurv = numCurvature;
unsigned int nLocals = numLocals;
unsigned int nBorder = nCurv + nLocals;
unsigned int nParBRL = nBorder + 2 * nDim;
unsigned int nParLoc = nLocals + 5;
std::vector<unsigned int> anIndex;
anIndex.reserve(nParBRL);
MatrixXd aJacobian(nParLoc, nParBRL);
aJacobian.setZero();
// find trajectory, position in trajectory
unsigned int aLabel = abs(aSignedLabel);
unsigned int firstLabel = 1;
unsigned int lastLabel = 0;
......@@ -552,11 +572,31 @@ std::pair<std::vector<unsigned int>, MatrixXd> GblTrajectory::getJacobian(
nJacobian = 1;
}
}
const GblPoint aPoint = thePoints[aTrajectory][aLabel - firstLabel];
std::array<unsigned int, 5> labDer;
Matrix5d matDer;
getFitToLocalJacobian(labDer, matDer, aPoint, 5, nJacobian);
unsigned int nBand = 0; // number of parameters from band part
if (numInnerTransformations > 0) {
unsigned int lastExt = innerTransLab[aTrajectory][2
* numInnerTransOffsets]; // last label for external parameters
for (unsigned int i = 0; i < 5; ++i)
if (labDer[i] > lastExt)
nBand++;
} else {
nBand = 2 * nDim;
}
unsigned int nBorder = nCurv + nLocals;
unsigned int nParBRL = nBorder + nBand;
unsigned int nParLoc = nLocals + 5;
std::vector<unsigned int> anIndex;
anIndex.reserve(nParBRL);
MatrixXd aJacobian(nParLoc, nParBRL);
aJacobian.setZero();
// from local parameters
for (unsigned int i = 0; i < nLocals; ++i) {
aJacobian(i + 5, i) = 1.0;
......@@ -564,13 +604,41 @@ std::pair<std::vector<unsigned int>, MatrixXd> GblTrajectory::getJacobian(
}
// from trajectory parameters
unsigned int iCol = nLocals;
for (unsigned int i = 0; i < 5; ++i) {
if (labDer[i] > 0) {
anIndex.push_back(labDer[i]);
for (unsigned int j = 0; j < 5; ++j) {
aJacobian(j, iCol) = matDer(j, i);
// composed trajectory ?
if (numInnerTransformations > 0) {
// transformation external to (simple) broken line (fit) parameters
unsigned int nSimple = nCurv + nBand;
MatrixXd aTrans(5, nSimple);
aTrans.setZero();
// external part, curvature
aTrans.block(0, 0, 1, nCurv) = innerTransDer[aTrajectory].block(0, 0, 1,
nCurv);
for (unsigned int i = 0; i < nCurv; ++i)
anIndex.push_back(++iCol);
if (nBand < 4) {
// external part, offsets (nBand=0: 2, nBand=2: 1)
unsigned int iRow = 1 + 2 * numInnerTransOffsets + nBand - 4; // start row (1: 1st, 3: 2nd offset)
aTrans.block(1, 0, 4 - nBand, nCurv) =
innerTransDer[aTrajectory].block(iRow, 0, 4 - nBand, nCurv);
}
// (remaining) band part
for (unsigned int i = 5 - nBand; i < 5; ++i) {
aTrans(i, iCol++) = 1.;
anIndex.push_back(
labDer[i]
- numInnerTransOffsets * nDim * (aTrajectory + 1)); // adjust label
}
aJacobian.block(0, nLocals, 5, nSimple) = matDer * aTrans;
} else {
// simple trajectory
for (unsigned int i = 0; i < 5; ++i) {
if (labDer[i] > 0) {
anIndex.push_back(labDer[i]);
for (unsigned int j = 0; j < 5; ++j) {
aJacobian(j, iCol) = matDer(j, i);
}
++iCol;
}
++iCol;
}
}
return std::make_pair(anIndex, aJacobian);
......@@ -1005,6 +1073,7 @@ void GblTrajectory::buildLinearEquationSystem() {
if (itData->getLabel() == skippedMeasLabel
&& itData->getType() == InternalMeasurement)
continue;
itData->getLocalData(aValue, aWeight, numLocal, indLocal, derLocal);
for (unsigned int j = 0; j < numLocal; ++j) {
theVector(indLocal[j] - 1) += derLocal[j] * aWeight * aValue;
......@@ -1016,6 +1085,10 @@ void GblTrajectory::buildLinearEquationSystem() {
/// Prepare fit for simple or composed trajectory
/**
* Generate data (blocks) from measurements, kinks, external seed and measurements.
*
* \exception 10 : inner transformation matrix with invalid number of rows (valid are 5=kinematic or 2=geometric constraint)
* \exception 11 : inner transformation matrix with to few columns (must be >= number of rows)
* \exception 12 : inner transformation matrices with varying sizes
*/
void GblTrajectory::prepare() {
unsigned int nDim = theDimension.size();
......@@ -1026,23 +1099,57 @@ void GblTrajectory::prepare() {
measDataIndex.resize(numAllPoints + 3); // include external seed and measurements
scatDataIndex.resize(numAllPoints + 1);
unsigned int nData = 0;
std::vector<MatrixXd> innerTransDer;
std::vector<std::array<unsigned int, 5> > innerTransLab;
// composed trajectory ?
if (numInnerTrans > 0) {
//std::cout << "composed trajectory" << std::endl;
if (numInnerTransformations > 0) {
unsigned int nInnerTransRows = innerTransformations[0].rows();
unsigned int nInnerTransCols = innerTransformations[0].cols();
// std::cout << "composed trajectory, inner transformation (" << nInnerTransRows << "," << nInnerTransCols << ")" << std::endl;
// check size
if (nInnerTransRows != 5 and nInnerTransRows != 2) {
std::cout
<< " GblTrajectory::prepare composed trajectory with bad inner transformation matrix("
<< nInnerTransRows << "," << nInnerTransCols
<< "): invalid number of rows" << std::endl;
throw 10;
}
if (nInnerTransRows > nInnerTransCols) {
std::cout
<< " GblTrajectory::prepare composed trajectory with bad inner transformation matrix("
<< nInnerTransRows << "," << nInnerTransCols
<< "): too few columns" << std::endl;
throw 11;
}
for (unsigned int iTraj = 0; iTraj < numTrajectories; ++iTraj) {
// check size
if (nInnerTransRows != innerTransformations[iTraj].rows()
or nInnerTransCols != innerTransformations[iTraj].cols()) {
std::cout
<< " GblTrajectory::prepare composed trajectory with bad inner transformation matrix["
<< iTraj << "]: different size as [0]" << std::endl;
throw 12;
}
// innermost point
GblPoint* innerPoint = &thePoints[iTraj].front();
// transformation fit to local track parameters
std::array<unsigned int, 5> firstLabels;
Matrix5d matFitToLocal, matLocalToFit;
Matrix5d matFitToLocal;
getFitToLocalJacobian(firstLabels, matFitToLocal, *innerPoint, 5);
// transformation local track to fit parameters
matLocalToFit = matFitToLocal.inverse();
// transformation external to fit parameters at inner (first) point
innerTransDer.emplace_back(
matLocalToFit * innerTransformations[iTraj]);
if (nInnerTransRows == 5) {
// (full) kinematic constraint
// transformation local track to fit parameters
Matrix5d matLocalToFit = matFitToLocal.inverse();
// transformation external to fit parameters at inner (first) point
innerTransDer.emplace_back(
matLocalToFit * innerTransformations[iTraj]);
} else {
// geometric constraint (including individual curvature correction per trajectory)
MatrixXd matInnerToFit(5, numCurvature);
matInnerToFit.setZero();
matInnerToFit(0, nInnerTransCols + iTraj) = 1.; // curvature correction for 'iTraj'
matInnerToFit.block(1, 0, 2, nInnerTransCols) =
innerTransformations[iTraj]; // geometric derivatives
innerTransDer.emplace_back(matInnerToFit);
}
innerTransLab.push_back(firstLabels);
}
}
......@@ -1083,22 +1190,24 @@ void GblTrajectory::prepare() {
* matDer.block<2, 5>(3, 0);
}
if (numInnerTrans > 0) {
if (numInnerTransformations > 0) {
// transform for external parameters
proDer.resize(measDim, Eigen::NoChange);
proDer.setZero();
// match parameters
unsigned int ifirst = 0;
unsigned int ilast = 2 * numInnerTransOffsets;
unsigned int ilabel = 0;
unsigned int numRelated = 0;
while (ilabel < 5) {
if (labDer[ilabel] > 0) {
while (innerTransLab[iTraj][ifirst]
!= labDer[ilabel] and ifirst < 5) {
!= labDer[ilabel] and ifirst <= ilast) {
++ifirst;
}