GblTrajectory.cpp 40.8 KB
Newer Older
1
2
3
4
5
6
7
/*
 * GblTrajectory.cpp
 *
 *  Created on: Aug 18, 2011
 *      Author: kleinwrt
 */

8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
/** \file
 *  GblTrajectory methods.
 *
 *  \author Claus Kleinwort, DESY, 2011 (Claus.Kleinwort@desy.de)
 *
 *  \copyright
 *  Copyright (c) 2011 - 2016 Deutsches Elektronen-Synchroton,
 *  Member of the Helmholtz Association, (DESY), HAMBURG, GERMANY \n\n
 *  This library is free software; you can redistribute it and/or modify
 *  it under the terms of the GNU Library General Public License as
 *  published by the Free Software Foundation; either version 2 of the
 *  License, or (at your option) any later version. \n\n
 *  This library is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU Library General Public License for more details. \n\n
 *  You should have received a copy of the GNU Library General Public
 *  License along with this program (see the file COPYING.LIB for more
 *  details); if not, write to the Free Software Foundation, Inc.,
 *  675 Mass Ave, Cambridge, MA 02139, USA.
 */

30
31
32
33
34
35
36
37
38
39
40
/** \mainpage General information
 *
 *  \section intro_sec Introduction
 *
 *  For a track with an initial trajectory from a prefit of the
 *  measurements (internal seed) or an external prediction
 *  (external seed) the description of multiple scattering is
 *  added by offsets in a local system. Along the initial
 *  trajectory points are defined with can describe a measurement
 *  or a (thin) scatterer or both. Measurements are arbitrary
 *  functions of the local track parameters at a point (e.g. 2D:
41
 *  position, 4D: direction+position). The refit provides corrections
42
43
 *  to the local track parameters (in the local system) and the
 *  corresponding covariance matrix at any of those points.
44
45
 *  Non-diagonal covariance matrices of
 *  measurements will be diagonalized internally.
46
47
 *  Outliers can be down-weighted by use of M-estimators.
 *
48
49
50
51
 *  A position measurement is in a plane defined by two directions.
 *  Along one direction the measurement precision may be zero,
 *  defining a 1D measurement in the other direction.
 *
52
53
54
55
56
57
58
 *  The broken lines trajectory is defined by (2D) offsets at the
 *  first and last point and all points with a scatterer. The
 *  prediction for a measurement is obtained by interpolation of
 *  the enclosing offsets and for triplets of adjacent offsets
 *  kink angles are determined. This requires for all points the
 *  jacobians for propagation to the previous and next offset.
 *  These are calculated from the point-to-point jacobians along
Claus Kleinwort's avatar
Claus Kleinwort committed
59
60
 *  the initial trajectory. The sequence of points has to be
 *  strictly monotonic in arc-length.
61
62
63
64
65
66
 *
 *  Additional local or global parameters can be added and the
 *  trajectories can be written to special binary files for
 *  calibration and alignment with Millepede-II.
 *  (V. Blobel, NIM A, 566 (2006), pp. 5-13).
 *
67
68
69
70
71
72
73
 *  Besides simple trajectories describing the path of a single
 *  particle composed trajectories are supported. These are
 *  constructed from the trajectories of multiple particles and
 *  some external parameters (like those describing a decay)
 *  and transformations at the first points from the external
 *  to the local track parameters.
 *
74
75
76
77
78
79
80
 *  The conventions for the coordinate systems follow:
 *  Derivation of Jacobians for the propagation of covariance
 *  matrices of track parameters in homogeneous magnetic fields
 *  A. Strandlie, W. Wittek, NIM A, 566 (2006) 687-698.
 *
 *  \section call_sec Calling sequence
 *
81
82
 *    -# Create list of points on initial trajectory:\n
 *            <tt>std::vector<GblPoint> list</tt>
83
84
 *    -# For all points on initial trajectory:
 *        - Create points and add appropriate attributes:\n
Claus Kleinwort's avatar
Claus Kleinwort committed
85
 *           - <tt>point = gbl::GblPoint(..)</tt>
86
 *           - <tt>point.addMeasurement(..)</tt>
87
88
89
 *           - Add additional local or global parameters to measurement:\n
 *             - <tt>point.addLocals(..)</tt>
 *             - <tt>point.addGlobals(..)</tt>
90
 *           - <tt>point.addScatterer(..)</tt>
91
92
 *        - Add point (ordered by arc length) to list:\n
 *            <tt>list.push_back(point)</tt>
93
 *    -# Create (simple) trajectory from list of points:\n
Claus Kleinwort's avatar
Claus Kleinwort committed
94
 *            <tt>traj = gbl::GblTrajectory (list)</tt>
95
 *    -# Optionally with external seed:\n
Claus Kleinwort's avatar
Claus Kleinwort committed
96
 *            <tt>traj = gbl::GblTrajectory (list,seed)</tt>
97
98
 *    -# Optionally check validity of trajectory:\n
 *            <tt>if (not traj.isValid()) .. //abort</tt>
99
 *    -# Fit trajectory, return error code,
100
101
 *       get Chi2, Ndf (and weight lost by M-estimators):\n
 *            <tt>ierr = traj.fit(..)</tt>
102
103
104
 *    -# For any point on initial trajectory:
 *        - Get corrections and covariance matrix for track parameters:\n
 *            <tt>[..] = traj.getResults(label)</tt>
Claus Kleinwort's avatar
Claus Kleinwort committed
105
 *    -# Optionally write trajectory to MP binary file (doesn't needs to be fitted):\n
106
107
 *            <tt>traj.milleOut(..)</tt>
 *
108
109
110
111
 *  \section loc_sec Local system and local parameters
 *  At each point on the trajectory a local coordinate system with local track
 *  parameters has to be defined. The first of the five parameters describes
 *  the bending, the next two the direction and the last two the position (offsets).
112
113
 *  The curvilinear system (T,U,V) with parameters (q/p, lambda, phi, x_t, y_t)
 *  is well suited.
114
 *
115
116
117
 *  \section impl_sec Implementation
 *
 *  Matrices are implemented with ROOT (root.cern.ch). User input or output is in the
118
119
 *  form of TMatrices. Internally SMatrices are used for fixes sized and simple matrices
 *  based on std::vector<> for variable sized matrices.
120
121
122
123
124
125
 *
 *  \section ref_sec References
 *    - V. Blobel, C. Kleinwort, F. Meier,
 *      Fast alignment of a complex tracking detector using advanced track models,
 *      Computer Phys. Communications (2011), doi:10.1016/j.cpc.2011.03.017
 *    - C. Kleinwort, General Broken Lines as advanced track fitting method,
126
 *      NIM A, 673 (2012), 107-110, doi:10.1016/j.nima.2012.01.024
127
128
129
130
 */

#include "GblTrajectory.h"

131
//! Namespace for the general broken lines package
Claus Kleinwort's avatar
Claus Kleinwort committed
132
namespace gbl {
133

134
/// Create new (simple) trajectory from list of points.
135
136
137
/**
 * Curved trajectory in space (default) or without curvature (q/p) or in one
 * plane (u-direction) only.
138
 * \param [in] aPointList List of points
139
140
141
142
 * \param [in] flagCurv Use q/p
 * \param [in] flagU1dir Use in u1 direction
 * \param [in] flagU2dir Use in u2 direction
 */
143
144
145
146
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(
Claus Kleinwort's avatar
Claus Kleinwort committed
147
				0), numMeasurements(0), externalPoint(0), theDimension(0), thePoints(), theData(), measDataIndex(), scatDataIndex(), externalSeed(), innerTransformations(), externalDerivatives(), externalMeasurements(), externalPrecisions() {
148

149
150
151
152
	if (flagU1dir)
		theDimension.push_back(0);
	if (flagU2dir)
		theDimension.push_back(1);
153
154
155
	// simple (single) trajectory
	thePoints.push_back(aPointList);
	numPoints.push_back(numAllPoints);
156
	construct(); // construct trajectory
157
158
}

159
/// Create new (simple) trajectory from list of points with external seed.
160
/**
161
162
163
164
165
166
167
168
169
 * Curved trajectory in space (default) or without curvature (q/p) or in one
 * plane (u-direction) only.
 * \param [in] aPointList List of points
 * \param [in] aLabel (Signed) label of point for external seed
 * (<0: in front, >0: after point, slope changes at scatterer!)
 * \param [in] aSeed Precision matrix of external seed
 * \param [in] flagCurv Use q/p
 * \param [in] flagU1dir Use in u1 direction
 * \param [in] flagU2dir Use in u2 direction
170
 */
171
GblTrajectory::GblTrajectory(const std::vector<GblPoint> &aPointList,
172
173
		unsigned int aLabel, const TMatrixDSym &aSeed, bool flagCurv,
		bool flagU1dir, bool flagU2dir) :
174
175
		numAllPoints(aPointList.size()), numPoints(), numOffsets(0), numInnerTrans(
				0), numCurvature(flagCurv ? 1 : 0), numParameters(0), numLocals(
Claus Kleinwort's avatar
Claus Kleinwort committed
176
				0), numMeasurements(0), externalPoint(aLabel), theDimension(0), thePoints(), theData(), measDataIndex(), scatDataIndex(), externalSeed(
177
				aSeed), innerTransformations(), externalDerivatives(), externalMeasurements(), externalPrecisions() {
178
179
180
181
182

	if (flagU1dir)
		theDimension.push_back(0);
	if (flagU2dir)
		theDimension.push_back(1);
183
184
185
	// simple (single) trajectory
	thePoints.push_back(aPointList);
	numPoints.push_back(numAllPoints);
186
	construct(); // construct trajectory
187
188
}

189
190
191
192
193
194
195
196
197
/// Create new composed trajectory from list of points and transformations.
/**
 * Composed of curved trajectory in space.
 * \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()), numParameters(0), numLocals(0), numMeasurements(
Claus Kleinwort's avatar
Claus Kleinwort committed
198
				0), externalPoint(0), theDimension(0), thePoints(), theData(), measDataIndex(), scatDataIndex(), externalSeed(), innerTransformations(), externalDerivatives(), externalMeasurements(), externalPrecisions() {
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225

	for (unsigned int iTraj = 0; iTraj < aPointsAndTransList.size(); ++iTraj) {
		thePoints.push_back(aPointsAndTransList[iTraj].first);
		numPoints.push_back(thePoints.back().size());
		numAllPoints += numPoints.back();
		innerTransformations.push_back(aPointsAndTransList[iTraj].second);
	}
	theDimension.push_back(0);
	theDimension.push_back(1);
	numCurvature = innerTransformations[0].GetNcols();
	construct(); // construct (composed) trajectory
}

/// Create new composed trajectory from list of points and transformations with (independent) external measurements.
/**
 * Composed of curved trajectory in space.
 * \param [in] aPointsAndTransList List containing pairs with list of points and transformation (at inner (first) point)
 * \param [in] extDerivatives Derivatives of external measurements vs external parameters
 * \param [in] extMeasurements External measurements (residuals)
 * \param [in] extPrecisions Precision of external measurements
 */
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()), numParameters(0), numLocals(0), numMeasurements(
Claus Kleinwort's avatar
Claus Kleinwort committed
226
				0), externalPoint(0), theDimension(0), thePoints(), theData(), measDataIndex(), scatDataIndex(), externalSeed(), innerTransformations(), externalDerivatives(
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
				extDerivatives), externalMeasurements(extMeasurements), externalPrecisions(
				extPrecisions) {

	for (unsigned int iTraj = 0; iTraj < aPointsAndTransList.size(); ++iTraj) {
		thePoints.push_back(aPointsAndTransList[iTraj].first);
		numPoints.push_back(thePoints.back().size());
		numAllPoints += numPoints.back();
		innerTransformations.push_back(aPointsAndTransList[iTraj].second);
	}
	theDimension.push_back(0);
	theDimension.push_back(1);
	numCurvature = innerTransformations[0].GetNcols();
	construct(); // construct (composed) trajectory
}

242
GblTrajectory::~GblTrajectory() {
243
244
}

245
246
247
248
249
/// Retrieve validity of trajectory
bool GblTrajectory::isValid() const {
	return constructOK;
}

250
/// Retrieve number of point from trajectory
251
unsigned int GblTrajectory::getNumPoints() const {
252
	return numAllPoints;
253
254
}

255
/// Construct trajectory from list of points.
256
/**
257
 * Trajectory is prepared for fit or output to binary file, may consists of sub-trajectories.
258
 */
259
260
void GblTrajectory::construct() {

261
	constructOK = false;
262
	fitOK = false;
263
	unsigned int aLabel = 0;
264
265
266
267
268
	if (numAllPoints < 2) {
		std::cout << " GblTrajectory construction failed: too few GblPoints "
				<< std::endl;
		return;
	}
269
270
	// loop over trajectories
	numTrajectories = thePoints.size();
271
	//std::cout << " numTrajectories: " << numTrajectories << ", " << innerTransformations.size() << std::endl;
272
273
274
275
276
277
278
279
	for (unsigned int iTraj = 0; iTraj < numTrajectories; ++iTraj) {
		std::vector<GblPoint>::iterator itPoint;
		for (itPoint = thePoints[iTraj].begin();
				itPoint < thePoints[iTraj].end(); ++itPoint) {
			numLocals = std::max(numLocals, itPoint->getNumLocals());
			numMeasurements += itPoint->hasMeasurement();
			itPoint->setLabel(++aLabel);
		}
280
281
282
	}
	defineOffsets();
	calcJacobians();
283
284
285
286
287
288
289
290
	try {
		prepare();
	} catch (std::overflow_error &e) {
		std::cout << " GblTrajectory construction failed: " << e.what()
				<< std::endl;
		return;
	}
	constructOK = true;
291
292
293
	// number of fit parameters
	numParameters = (numOffsets - 2 * numInnerTrans) * theDimension.size()
			+ numCurvature + numLocals;
294
295
296
297
298
299
300
301
}

/// Define offsets from list of points.
/**
 * Define offsets at points with scatterers and first and last point.
 * All other points need interpolation from adjacent points with offsets.
 */
void GblTrajectory::defineOffsets() {
302
303
304
305
306
307
308
309
310
311
312
313
314
315

	// loop over trajectories
	for (unsigned int iTraj = 0; iTraj < numTrajectories; ++iTraj) {
		// first point is offset
		thePoints[iTraj].front().setOffset(numOffsets++);
		// intermediate scatterers are offsets
		std::vector<GblPoint>::iterator itPoint;
		for (itPoint = thePoints[iTraj].begin() + 1;
				itPoint < thePoints[iTraj].end() - 1; ++itPoint) {
			if (itPoint->hasScatterer()) {
				itPoint->setOffset(numOffsets++);
			} else {
				itPoint->setOffset(-numOffsets);
			}
316
		}
317
318
		// last point is offset
		thePoints[iTraj].back().setOffset(numOffsets++);
319
320
321
322
323
324
325
	}
}

/// Calculate Jacobians to previous/next scatterer from point to point ones.
void GblTrajectory::calcJacobians() {

	SMatrix55 scatJacobian;
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
	// loop over trajectories
	for (unsigned int iTraj = 0; iTraj < numTrajectories; ++iTraj) {
		// forward propagation (all)
		GblPoint* previousPoint = &thePoints[iTraj].front();
		unsigned int numStep = 0;
		std::vector<GblPoint>::iterator itPoint;
		for (itPoint = thePoints[iTraj].begin() + 1;
				itPoint < thePoints[iTraj].end(); ++itPoint) {
			if (numStep == 0) {
				scatJacobian = itPoint->getP2pJacobian();
			} else {
				scatJacobian = itPoint->getP2pJacobian() * scatJacobian;
			}
			numStep++;
			itPoint->addPrevJacobian(scatJacobian); // iPoint -> previous scatterer
			if (itPoint->getOffset() >= 0) {
				previousPoint->addNextJacobian(scatJacobian); // lastPoint -> next scatterer
				numStep = 0;
				previousPoint = &(*itPoint);
			}
346
		}
347
348
		// backward propagation (without scatterers)
		for (itPoint = thePoints[iTraj].end() - 1;
349
				itPoint > thePoints[iTraj].begin(); --itPoint) {
350
351
			if (itPoint->getOffset() >= 0) {
				scatJacobian = itPoint->getP2pJacobian();
352
				continue; // skip offsets
353
354
			}
			itPoint->addNextJacobian(scatJacobian); // iPoint -> next scatterer
355
			scatJacobian = scatJacobian * itPoint->getP2pJacobian();
356
357
358
359
360
361
362
363
364
365
366
367
368
369
		}
	}
}

/// 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.
 * \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
 * corresponding transformation matrix
 */
std::pair<std::vector<unsigned int>, TMatrixD> GblTrajectory::getJacobian(
370
		int aSignedLabel) const {
371
372
373
374
375
376
377
378
379
380
381
382
383

	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);
	TMatrixD aJacobian(nParLoc, nParBRL);
	aJacobian.Zero();

	unsigned int aLabel = abs(aSignedLabel);
384
385
386
387
388
389
390
391
392
393
394
395
	unsigned int firstLabel = 1;
	unsigned int lastLabel = 0;
	unsigned int aTrajectory = 0;
	// loop over trajectories
	for (unsigned int iTraj = 0; iTraj < numTrajectories; ++iTraj) {
		aTrajectory = iTraj;
		lastLabel += numPoints[iTraj];
		if (aLabel <= lastLabel)
			break;
		if (iTraj < numTrajectories - 1)
			firstLabel += numPoints[iTraj];
	}
396
397
398
399
	int nJacobian; // 0: prev, 1: next
	// check consistency of (index, direction)
	if (aSignedLabel > 0) {
		nJacobian = 1;
400
401
		if (aLabel >= lastLabel) {
			aLabel = lastLabel;
402
403
404
405
			nJacobian = 0;
		}
	} else {
		nJacobian = 0;
406
407
		if (aLabel <= firstLabel) {
			aLabel = firstLabel;
408
409
410
			nJacobian = 1;
		}
	}
411
	const GblPoint aPoint = thePoints[aTrajectory][aLabel - firstLabel];
412
413
414
415
416
	std::vector<unsigned int> labDer(5);
	SMatrix55 matDer;
	getFitToLocalJacobian(labDer, matDer, aPoint, 5, nJacobian);

	// from local parameters
417
	for (unsigned int i = 0; i < nLocals; ++i) {
418
419
420
421
422
		aJacobian(i + 5, i) = 1.0;
		anIndex.push_back(i + 1);
	}
	// from trajectory parameters
	unsigned int iCol = nLocals;
423
	for (unsigned int i = 0; i < 5; ++i) {
424
425
		if (labDer[i] > 0) {
			anIndex.push_back(labDer[i]);
426
			for (unsigned int j = 0; j < 5; ++j) {
427
428
				aJacobian(j, iCol) = matDer(j, i);
			}
429
			++iCol;
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
		}
	}
	return std::make_pair(anIndex, aJacobian);
}

/// Get (part of) jacobian for transformation from (trajectory) fit to track parameters at point.
/**
 * Jacobian broken lines (q/p,..,u_i,u_i+1..) to local (q/p,u',u) parameters.
 * \param [out] anIndex List of fit parameters with non zero derivatives
 * \param [out] aJacobian Corresponding transformation matrix
 * \param [in] aPoint Point to use
 * \param [in] measDim Dimension of 'measurement'
 * (<=2: calculate only offset part, >2: complete matrix)
 * \param [in] nJacobian Direction (0: to previous offset, 1: to next offset)
 */
void GblTrajectory::getFitToLocalJacobian(std::vector<unsigned int> &anIndex,
446
		SMatrix55 &aJacobian, const GblPoint &aPoint, unsigned int measDim,
447
		unsigned int nJacobian) const {
448
449
450
451
452
453
454
455
456

	unsigned int nDim = theDimension.size();
	unsigned int nCurv = numCurvature;
	unsigned int nLocals = numLocals;

	int nOffset = aPoint.getOffset();

	if (nOffset < 0) // need interpolation
			{
457
458
		SMatrix22 prevW, prevWJ, nextW, nextWJ, matN;
		SVector2 prevWd, nextWd;
459
460
461
		int ierr;
		aPoint.getDerivatives(0, prevW, prevWJ, prevWd); // W-, W- * J-, W- * d-
		aPoint.getDerivatives(1, nextW, nextWJ, nextWd); // W-, W- * J-, W- * d-
462
		const SMatrix22 sumWJ(prevWJ + nextWJ);
463
464
		matN = sumWJ.Inverse(ierr); // N = (W- * J- + W+ * J+)^-1
		// derivatives for u_int
465
466
467
468
		const SMatrix22 prevNW(matN * prevW); // N * W-
		const SMatrix22 nextNW(matN * nextW); // N * W+
		const SVector2 prevNd(matN * prevWd); // N * W- * d-
		const SVector2 nextNd(matN * nextWd); // N * W+ * d+
469
470
471
472
473
474
475
476
477
478

		unsigned int iOff = nDim * (-nOffset - 1) + nLocals + nCurv + 1; // first offset ('i' in u_i)

		// local offset
		if (nCurv > 0) {
			aJacobian.Place_in_col(-prevNd - nextNd, 3, 0); // from curvature
			anIndex[0] = nLocals + 1;
		}
		aJacobian.Place_at(prevNW, 3, 1); // from 1st Offset
		aJacobian.Place_at(nextNW, 3, 3); // from 2nd Offset
479
		for (unsigned int i = 0; i < nDim; ++i) {
480
481
482
483
484
485
486
			anIndex[1 + theDimension[i]] = iOff + i;
			anIndex[3 + theDimension[i]] = iOff + nDim + i;
		}

		// local slope and curvature
		if (measDim > 2) {
			// derivatives for u'_int
487
488
489
490
			const SMatrix22 prevWPN(nextWJ * prevNW); // W+ * J+ * N * W-
			const SMatrix22 nextWPN(prevWJ * nextNW); // W- * J- * N * W+
			const SVector2 prevWNd(nextWJ * prevNd); // W+ * J+ * N * W- * d-
			const SVector2 nextWNd(prevWJ * nextNd); // W- * J- * N * W+ * d+
491
492
493
494
495
496
497
			if (nCurv > 0) {
				aJacobian(0, 0) = 1.0;
				aJacobian.Place_in_col(prevWNd - nextWNd, 1, 0); // from curvature
			}
			aJacobian.Place_at(-prevWPN, 1, 1); // from 1st Offset
			aJacobian.Place_at(nextWPN, 1, 3); // from 2nd Offset
		}
498
499
500
501
502
503
504
505
	} else { // at point
		// anIndex must be sorted
		// forward : iOff2 = iOff1 + nDim, index1 = 1, index2 = 3
		// backward: iOff2 = iOff1 - nDim, index1 = 3, index2 = 1
		unsigned int iOff1 = nDim * nOffset + nCurv + nLocals + 1; // first offset ('i' in u_i)
		unsigned int index1 = 3 - 2 * nJacobian; // index of first offset
		unsigned int iOff2 = iOff1 + nDim * (nJacobian * 2 - 1); // second offset ('i' in u_i)
		unsigned int index2 = 1 + 2 * nJacobian; // index of second offset
506
		// local offset
507
508
		aJacobian(3, index1) = 1.0; // from 1st Offset
		aJacobian(4, index1 + 1) = 1.0;
509
		for (unsigned int i = 0; i < nDim; ++i) {
510
			anIndex[index1 + theDimension[i]] = iOff1 + i;
511
512
513
514
515
516
517
		}

		// local slope and curvature
		if (measDim > 2) {
			SMatrix22 matW, matWJ;
			SVector2 vecWd;
			aPoint.getDerivatives(nJacobian, matW, matWJ, vecWd); // W, W * J, W * d
518
			double sign = (nJacobian > 0) ? 1. : -1.;
519
520
			if (nCurv > 0) {
				aJacobian(0, 0) = 1.0;
521
				aJacobian.Place_in_col(-sign * vecWd, 1, 0); // from curvature
522
523
				anIndex[0] = nLocals + 1;
			}
524
525
			aJacobian.Place_at(-sign * matWJ, 1, index1); // from 1st Offset
			aJacobian.Place_at(sign * matW, 1, index2); // from 2nd Offset
526
			for (unsigned int i = 0; i < nDim; ++i) {
527
				anIndex[index2 + theDimension[i]] = iOff2 + i;
528
529
530
531
532
533
534
535
536
537
538
539
540
			}
		}
	}
}

/// Get jacobian for transformation from (trajectory) fit to kink parameters at point.
/**
 * Jacobian broken lines (q/p,..,u_i-1,u_i,u_i+1..) to kink (du') parameters.
 * \param [out] anIndex List of fit parameters with non zero derivatives
 * \param [out] aJacobian Corresponding transformation matrix
 * \param [in] aPoint Point to use
 */
void GblTrajectory::getFitToKinkJacobian(std::vector<unsigned int> &anIndex,
541
		SMatrix27 &aJacobian, const GblPoint &aPoint) const {
542
543
544
545
546
547
548

	unsigned int nDim = theDimension.size();
	unsigned int nCurv = numCurvature;
	unsigned int nLocals = numLocals;

	int nOffset = aPoint.getOffset();

549
550
	SMatrix22 prevW, prevWJ, nextW, nextWJ;
	SVector2 prevWd, nextWd;
551
552
	aPoint.getDerivatives(0, prevW, prevWJ, prevWd); // W-, W- * J-, W- * d-
	aPoint.getDerivatives(1, nextW, nextWJ, nextWd); // W-, W- * J-, W- * d-
553
554
	const SMatrix22 sumWJ(prevWJ + nextWJ); // W- * J- + W+ * J+
	const SVector2 sumWd(prevWd + nextWd); // W+ * d+ + W- * d-
555
556
557
558
559
560
561
562
563
564
565

	unsigned int iOff = (nOffset - 1) * nDim + nCurv + nLocals + 1; // first offset ('i' in u_i)

	// local offset
	if (nCurv > 0) {
		aJacobian.Place_in_col(-sumWd, 0, 0); // from curvature
		anIndex[0] = nLocals + 1;
	}
	aJacobian.Place_at(prevW, 0, 1); // from 1st Offset
	aJacobian.Place_at(-sumWJ, 0, 3); // from 2nd Offset
	aJacobian.Place_at(nextW, 0, 5); // from 1st Offset
566
	for (unsigned int i = 0; i < nDim; ++i) {
567
568
569
570
571
572
573
574
575
576
		anIndex[1 + theDimension[i]] = iOff + i;
		anIndex[3 + theDimension[i]] = iOff + nDim + i;
		anIndex[5 + theDimension[i]] = iOff + nDim * 2 + i;
	}
}

/// Get fit results at point.
/**
 * Get corrections and covariance matrix for local track and additional parameters
 * in forward or backward direction.
577
 * \param [in] aSignedLabel (Signed) label of point on trajectory
578
579
580
 * (<0: in front, >0: after point, slope changes at scatterer!)
 * \param [out] localPar Corrections for local parameters
 * \param [out] localCov Covariance for local parameters
581
 * \return error code (non-zero if trajectory not fitted successfully)
582
 */
583
unsigned int GblTrajectory::getResults(int aSignedLabel, TVectorD &localPar,
584
		TMatrixDSym &localCov) const {
585
586
	if (not fitOK)
		return 1;
587
588
589
590
	std::pair<std::vector<unsigned int>, TMatrixD> indexAndJacobian =
			getJacobian(aSignedLabel);
	unsigned int nParBrl = indexAndJacobian.first.size();
	TVectorD aVec(nParBrl); // compressed vector
591
	for (unsigned int i = 0; i < nParBrl; ++i) {
592
		aVec[i] = theVector(indexAndJacobian.first[i] - 1);
593
594
595
596
	}
	TMatrixDSym aMat = theMatrix.getBlockMatrix(indexAndJacobian.first); // compressed matrix
	localPar = indexAndJacobian.second * aVec;
	localCov = aMat.Similarity(indexAndJacobian.second);
597
598
599
600
601
	return 0;
}

/// Get residuals at point from measurement.
/**
602
 * Get (diagonalized) residual, error of measurement and residual and down-weighting
603
 * factor for measurement at point
604
 *
605
 * \param [in]  aLabel Label of point on trajectory
606
607
608
609
610
611
612
613
614
615
616
617
618
619
620
621
622
623
624
625
626
627
628
629
630
 * \param [out] numData Number of data blocks from measurement at point
 * \param [out] aResiduals Measurements-Predictions
 * \param [out] aMeasErrors Errors of Measurements
 * \param [out] aResErrors Errors of Residuals (including correlations from track fit)
 * \param [out] aDownWeights Down-Weighting factors
 * \return error code (non-zero if trajectory not fitted successfully)
 */
unsigned int GblTrajectory::getMeasResults(unsigned int aLabel,
		unsigned int &numData, TVectorD &aResiduals, TVectorD &aMeasErrors,
		TVectorD &aResErrors, TVectorD &aDownWeights) {
	numData = 0;
	if (not fitOK)
		return 1;

	unsigned int firstData = measDataIndex[aLabel - 1]; // first data block with measurement
	numData = measDataIndex[aLabel] - firstData; // number of data blocks
	for (unsigned int i = 0; i < numData; ++i) {
		getResAndErr(firstData + i, aResiduals[i], aMeasErrors[i],
				aResErrors[i], aDownWeights[i]);
	}
	return 0;
}

/// Get (kink) residuals at point from scatterer.
/**
631
 * Get (diagonalized) residual, error of measurement and residual and down-weighting
632
 * factor for scatterering kinks at point
633
 *
634
 * \param [in]  aLabel Label of point on trajectory
635
636
637
638
639
640
641
642
643
644
645
646
647
648
649
650
651
652
653
654
655
656
657
 * \param [out] numData Number of data blocks from scatterer at point
 * \param [out] aResiduals (kink)Measurements-(kink)Predictions
 * \param [out] aMeasErrors Errors of (kink)Measurements
 * \param [out] aResErrors Errors of Residuals (including correlations from track fit)
 * \param [out] aDownWeights Down-Weighting factors
 * \return error code (non-zero if trajectory not fitted successfully)
 */
unsigned int GblTrajectory::getScatResults(unsigned int aLabel,
		unsigned int &numData, TVectorD &aResiduals, TVectorD &aMeasErrors,
		TVectorD &aResErrors, TVectorD &aDownWeights) {
	numData = 0;
	if (not fitOK)
		return 1;

	unsigned int firstData = scatDataIndex[aLabel - 1]; // first data block with scatterer
	numData = scatDataIndex[aLabel] - firstData; // number of data blocks
	for (unsigned int i = 0; i < numData; ++i) {
		getResAndErr(firstData + i, aResiduals[i], aMeasErrors[i],
				aResErrors[i], aDownWeights[i]);
	}
	return 0;
}

658
659
/// Get (list of) labels of points on (simple) trajectory
/**
Claus Kleinwort's avatar
Claus Kleinwort committed
660
 * \param [out] aLabelList List of labels (aLabelList[i] = i+1)
661
 */
Claus Kleinwort's avatar
Claus Kleinwort committed
662
void GblTrajectory::getLabels(std::vector<unsigned int> &aLabelList) {
663
664
665
666
667
668
669
670
671
672
	unsigned int aLabel = 0;
	unsigned int nPoint = thePoints[0].size();
	aLabelList.resize(nPoint);
	for (unsigned i = 0; i < nPoint; ++i) {
		aLabelList[i] = ++aLabel;
	}
}

/// Get (list of lists of) labels of points on (composed) trajectory
/**
Claus Kleinwort's avatar
Claus Kleinwort committed
673
 * \param [out] aLabelList List of of lists of labels
674
675
 */
void GblTrajectory::getLabels(
Claus Kleinwort's avatar
Claus Kleinwort committed
676
		std::vector<std::vector<unsigned int> > &aLabelList) {
677
678
679
680
681
682
683
684
685
686
687
	unsigned int aLabel = 0;
	aLabelList.resize(numTrajectories);
	for (unsigned int iTraj = 0; iTraj < numTrajectories; ++iTraj) {
		unsigned int nPoint = thePoints[iTraj].size();
		aLabelList[iTraj].resize(nPoint);
		for (unsigned i = 0; i < nPoint; ++i) {
			aLabelList[iTraj][i] = ++aLabel;
		}
	}
}

688
689
690
691
692
693
694
695
696
697
698
699
700
701
702
703
704
705
706
707
708
709
710
711
712
713
714
/// Get residual and errors from data block.
/**
 * Get residual, error of measurement and residual and down-weighting
 * factor for (single) data block
 * \param [in]  aData Label of data block
 * \param [out] aResidual Measurement-Prediction
 * \param [out] aMeasError Error of Measurement
 * \param [out] aResError Error of Residual (including correlations from track fit)
 * \param [out] aDownWeight Down-Weighting factor
 */
void GblTrajectory::getResAndErr(unsigned int aData, double &aResidual,
		double &aMeasError, double &aResError, double &aDownWeight) {

	double aMeasVar;
	std::vector<unsigned int>* indLocal;
	std::vector<double>* derLocal;
	theData[aData].getResidual(aResidual, aMeasVar, aDownWeight, indLocal,
			derLocal);
	unsigned int nParBrl = (*indLocal).size();
	TVectorD aVec(nParBrl); // compressed vector of derivatives
	for (unsigned int j = 0; j < nParBrl; ++j) {
		aVec[j] = (*derLocal)[j];
	}
	TMatrixDSym aMat = theMatrix.getBlockMatrix(*indLocal); // compressed (covariance) matrix
	double aFitVar = aMat.Similarity(aVec); // variance from track fit
	aMeasError = sqrt(aMeasVar); // error of measurement
	aResError = (aFitVar < aMeasVar ? sqrt(aMeasVar - aFitVar) : 0.); // error of residual
715
716
717
718
719
}

/// Build linear equation system from data (blocks).
void GblTrajectory::buildLinearEquationSystem() {
	unsigned int nBorder = numCurvature + numLocals;
720
	theVector.resize(numParameters);
721
722
723
724
725
	theMatrix.resize(numParameters, nBorder);
	double aValue, aWeight;
	std::vector<unsigned int>* indLocal;
	std::vector<double>* derLocal;
	std::vector<GblData>::iterator itData;
726
	for (itData = theData.begin(); itData < theData.end(); ++itData) {
727
		itData->getLocalData(aValue, aWeight, indLocal, derLocal);
728
		for (unsigned int j = 0; j < indLocal->size(); ++j) {
729
			theVector((*indLocal)[j] - 1) += (*derLocal)[j] * aWeight * aValue;
730
731
732
733
734
		}
		theMatrix.addBlockMatrix(aWeight, indLocal, derLocal);
	}
}

735
/// Prepare fit for simple or composed trajectory
736
/**
737
 * Generate data (blocks) from measurements, kinks, external seed and measurements.
738
739
740
 */
void GblTrajectory::prepare() {
	unsigned int nDim = theDimension.size();
741
742
743
	// upper limit
	unsigned int maxData = numMeasurements + nDim * (numOffsets - 2)
			+ externalSeed.GetNrows();
744
	theData.reserve(maxData);
745
746
	measDataIndex.resize(numAllPoints + 3); // include external seed and measurements
	scatDataIndex.resize(numAllPoints + 1);
747
	unsigned int nData = 0;
748
749
750
751
752
753
754
755
756
757
758
759
760
761
762
763
764
765
766
767
768
769
770
771
772
773
	std::vector<TMatrixD> innerTransDer;
	std::vector<std::vector<unsigned int> > innerTransLab;
	// composed trajectory ?
	if (numInnerTrans > 0) {
		//std::cout << "composed trajectory" << std::endl;
		for (unsigned int iTraj = 0; iTraj < numTrajectories; ++iTraj) {
			// innermost point
			GblPoint* innerPoint = &thePoints[iTraj].front();
			// transformation fit to local track parameters
			std::vector<unsigned int> firstLabels(5);
			SMatrix55 matFitToLocal, matLocalToFit;
			getFitToLocalJacobian(firstLabels, matFitToLocal, *innerPoint, 5);
			// transformation local track to fit parameters
			int ierr;
			matLocalToFit = matFitToLocal.Inverse(ierr);
			TMatrixD localToFit(5, 5);
			for (unsigned int i = 0; i < 5; ++i) {
				for (unsigned int j = 0; j < 5; ++j) {
					localToFit(i, j) = matLocalToFit(i, j);
				}
			}
			// transformation external to fit parameters at inner (first) point
			innerTransDer.push_back(localToFit * innerTransformations[iTraj]);
			innerTransLab.push_back(firstLabels);
		}
	}
774
775
	// measurements
	SMatrix55 matP;
776
	// loop over trajectories
777
	std::vector<GblPoint>::iterator itPoint;
778
779
780
781
782
783
784
785
786
787
788
789
790
791
792
	for (unsigned int iTraj = 0; iTraj < numTrajectories; ++iTraj) {
		for (itPoint = thePoints[iTraj].begin();
				itPoint < thePoints[iTraj].end(); ++itPoint) {
			SVector5 aMeas, aPrec;
			unsigned int nLabel = itPoint->getLabel();
			unsigned int measDim = itPoint->hasMeasurement();
			if (measDim) {
				const TMatrixD localDer = itPoint->getLocalDerivatives();
				const std::vector<int> globalLab = itPoint->getGlobalLabels();
				const TMatrixD globalDer = itPoint->getGlobalDerivatives();
				TMatrixD transDer;
				itPoint->getMeasurement(matP, aMeas, aPrec);
				unsigned int iOff = 5 - measDim; // first active component
				std::vector<unsigned int> labDer(5);
				SMatrix55 matDer, matPDer;
793
794
795
796
				unsigned int nJacobian =
						(itPoint < thePoints[iTraj].end() - 1) ? 1 : 0; // last point needs backward propagation
				getFitToLocalJacobian(labDer, matDer, *itPoint, measDim,
						nJacobian);
797
798
799
800
801
802
803
				if (measDim > 2) {
					matPDer = matP * matDer;
				} else { // 'shortcut' for position measurements
					matPDer.Place_at(
							matP.Sub<SMatrix22>(3, 3)
									* matDer.Sub<SMatrix25>(3, 0), 3, 0);
				}
804

805
806
807
808
809
810
811
812
813
814
815
816
817
818
819
820
821
822
823
824
825
826
827
828
829
830
831
832
833
834
835
836
837
838
839
840
				if (numInnerTrans > 0) {
					// transform for external parameters
					TMatrixD proDer(measDim, 5);
					// match parameters
					unsigned int ifirst = 0;
					unsigned int ilabel = 0;
					while (ilabel < 5) {
						if (labDer[ilabel] > 0) {
							while (innerTransLab[iTraj][ifirst]
									!= labDer[ilabel] and ifirst < 5) {
								++ifirst;
							}
							if (ifirst >= 5) {
								labDer[ilabel] -= 2 * nDim * (iTraj + 1); // adjust label
							} else {
								// match
								labDer[ilabel] = 0; // mark as related to external parameters
								for (unsigned int k = iOff; k < 5; ++k) {
									proDer(k - iOff, ifirst) = matPDer(k,
											ilabel);
								}
							}
						}
						++ilabel;
					}
					transDer.ResizeTo(measDim, numCurvature);
					transDer = proDer * innerTransDer[iTraj];
				}
				for (unsigned int i = iOff; i < 5; ++i) {
					if (aPrec(i) > 0.) {
						GblData aData(nLabel, aMeas(i), aPrec(i));
						aData.addDerivatives(i, labDer, matPDer, iOff, localDer,
								globalLab, globalDer, numLocals, transDer);
						theData.push_back(aData);
						nData++;
					}
841
842
				}

843
844
			}
			measDataIndex[nLabel] = nData;
845
846
		}
	}
847

848
	// pseudo measurements from kinks
849
	SMatrix22 matT;
850
851
	scatDataIndex[0] = nData;
	scatDataIndex[1] = nData;
852
853
854
855
856
857
858
	// loop over trajectories
	for (unsigned int iTraj = 0; iTraj < numTrajectories; ++iTraj) {
		for (itPoint = thePoints[iTraj].begin() + 1;
				itPoint < thePoints[iTraj].end() - 1; ++itPoint) {
			SVector2 aMeas, aPrec;
			unsigned int nLabel = itPoint->getLabel();
			if (itPoint->hasScatterer()) {
859
				itPoint->getScatterer(matT, aMeas, aPrec);
860
861
				TMatrixD transDer;
				std::vector<unsigned int> labDer(7);
862
				SMatrix27 matDer, matTDer;
863
				getFitToKinkJacobian(labDer, matDer, *itPoint);
864
				matTDer = matT * matDer;
865
866
867
868
869
870
871
872
873
874
875
876
877
878
879
880
881
882
				if (numInnerTrans > 0) {
					// transform for external parameters
					TMatrixD proDer(nDim, 5);
					// match parameters
					unsigned int ifirst = 0;
					unsigned int ilabel = 0;
					while (ilabel < 7) {
						if (labDer[ilabel] > 0) {
							while (innerTransLab[iTraj][ifirst]
									!= labDer[ilabel] and ifirst < 5) {
								++ifirst;
							}
							if (ifirst >= 5) {
								labDer[ilabel] -= 2 * nDim * (iTraj + 1); // adjust label
							} else {
								// match
								labDer[ilabel] = 0; // mark as related to external parameters
								for (unsigned int k = 0; k < nDim; ++k) {
883
									proDer(k, ifirst) = matTDer(k, ilabel);
884
885
886
887
888
889
890
891
892
893
894
895
								}
							}
						}
						++ilabel;
					}
					transDer.ResizeTo(nDim, numCurvature);
					transDer = proDer * innerTransDer[iTraj];
				}
				for (unsigned int i = 0; i < nDim; ++i) {
					unsigned int iDim = theDimension[i];
					if (aPrec(iDim) > 0.) {
						GblData aData(nLabel, aMeas(iDim), aPrec(iDim));
896
						aData.addDerivatives(iDim, labDer, matTDer, numLocals,
897
898
899
900
								transDer);
						theData.push_back(aData);
						nData++;
					}
901
902
				}
			}
903
			scatDataIndex[nLabel] = nData;
904
		}
905
		scatDataIndex[thePoints[iTraj].back().getLabel()] = nData;
906
	}
907

908
909
910
911
	// external seed
	if (externalPoint > 0) {
		std::pair<std::vector<unsigned int>, TMatrixD> indexAndJacobian =
				getJacobian(externalPoint);
Claus Kleinwort's avatar
Claus Kleinwort committed
912
913
914
915
916
		std::vector<unsigned int> externalSeedIndex = indexAndJacobian.first;
		std::vector<double> externalSeedDerivatives(externalSeedIndex.size());
		const TMatrixDSymEigen externalSeedEigen(externalSeed);
		const TVectorD valEigen = externalSeedEigen.GetEigenValues();
		TMatrixD vecEigen = externalSeedEigen.GetEigenVectors();
917
		vecEigen = vecEigen.T() * indexAndJacobian.second;
918
		for (int i = 0; i < externalSeed.GetNrows(); ++i) {
919
			if (valEigen(i) > 0.) {
920
				for (int j = 0; j < externalSeed.GetNcols(); ++j) {
Claus Kleinwort's avatar
Claus Kleinwort committed
921
					externalSeedDerivatives[j] = vecEigen(i, j);
922
923
				}
				GblData aData(externalPoint, 0., valEigen(i));
Claus Kleinwort's avatar
Claus Kleinwort committed
924
				aData.addDerivatives(externalSeedIndex, externalSeedDerivatives);
925
				theData.push_back(aData);
926
927
928
929
				nData++;
			}
		}
	}
930
	measDataIndex[numAllPoints + 1] = nData;
931
932
933
934
935
936
937
938
939
	// external measurements
	unsigned int nExt = externalMeasurements.GetNrows();
	if (nExt > 0) {
		std::vector<unsigned int> index(numCurvature);
		std::vector<double> derivatives(numCurvature);
		for (unsigned int iExt = 0; iExt < nExt; ++iExt) {
			for (unsigned int iCol = 0; iCol < numCurvature; ++iCol) {
				index[iCol] = iCol + 1;
				derivatives[iCol] = externalDerivatives(iExt, iCol);
940
			}
941
942
943
944
945
			GblData aData(1U, externalMeasurements(iExt),
					externalPrecisions(iExt));
			aData.addDerivatives(index, derivatives);
			theData.push_back(aData);
			nData++;
946
947
		}
	}
948
	measDataIndex[numAllPoints + 2] = nData;
949
950
951
952
953
}

/// Calculate predictions for all points.
void GblTrajectory::predict() {
	std::vector<GblData>::iterator itData;
954
	for (itData = theData.begin(); itData < theData.end(); ++itData) {
955
956
957
958
959
960
961
962
963
964
965
		itData->setPrediction(theVector);
	}
}

/// Down-weight all points.
/**
 * \param [in] aMethod M-estimator (1: Tukey, 2:Huber, 3:Cauchy)
 */
double GblTrajectory::downWeight(unsigned int aMethod) {
	double aLoss = 0.;
	std::vector<GblData>::iterator itData;
966
	for (itData = theData.begin(); itData < theData.end(); ++itData) {
967
968
969
970
971
972
973
974
		aLoss += (1. - itData->setDownWeighting(aMethod));
	}
	return aLoss;
}

/// Perform fit of trajectory.
/**
 * Optionally iterate for outlier down-weighting.
975
976
 * Fit may fail due to singular or not positive definite matrices (internal exceptions 1-3).
 *
977
978
979
980
981
 * \param [out] Chi2 Chi2 sum (corrected for down-weighting)
 * \param [out] Ndf  Number of degrees of freedom
 * \param [out] lostWeight Sum of weights lost due to down-weighting
 * \param [in] optionList Iterations for down-weighting
 * (One character per iteration: t,h,c (or T,H,C) for Tukey, Huber or Cauchy function)
982
 * \return Error code (non zero value indicates failure of fit)
983
 */
984
unsigned int GblTrajectory::fit(double &Chi2, int &Ndf, double &lostWeight,
985
986
987
988
		std::string optionList) {
	const double normChi2[4] = { 1.0, 0.8737, 0.9326, 0.8228 };
	const std::string methodList = "TtHhCc";

989
990
991
992
993
994
	Chi2 = 0.;
	Ndf = -1;
	lostWeight = 0.;
	if (not constructOK)
		return 10;

995
996
997
998
	unsigned int aMethod = 0;

	buildLinearEquationSystem();
	lostWeight = 0.;
999
1000
	unsigned int ierr = 0;
	try {
For faster browsing, not all history is shown. View entire blame