GblPoint.cpp 15.9 KB
Newer Older
1
2
3
4
5
6
7
/*
 * GblPoint.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
 *  GblPoint 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
#include "GblPoint.h"

32
//! Namespace for the general broken lines package
Claus Kleinwort's avatar
Claus Kleinwort committed
33
namespace gbl {
34

35
36
37
38
39
40
41
42
/// Create a point.
/**
 * Create point on (initial) trajectory. Needs transformation jacobian from previous point.
 * \param [in] aJacobian Transformation jacobian from previous point
 */
GblPoint::GblPoint(const TMatrixD &aJacobian) :
		theLabel(0), theOffset(0), measDim(0), transFlag(false), measTransformation(), scatFlag(
				false), localDerivatives(), globalLabels(), globalDerivatives() {
43

44
45
	for (unsigned int i = 0; i < 5; ++i) {
		for (unsigned int j = 0; j < 5; ++j) {
46
47
48
49
50
			p2pJacobian(i, j) = aJacobian(i, j);
		}
	}
}

Claus Kleinwort's avatar
Claus Kleinwort committed
51
52
53
54
55
56
GblPoint::GblPoint(const SMatrix55 &aJacobian) :
		theLabel(0), theOffset(0), p2pJacobian(aJacobian), measDim(0), transFlag(
				false), measTransformation(), scatFlag(false), localDerivatives(), globalLabels(), globalDerivatives() {

}

57
58
59
GblPoint::~GblPoint() {
}

60
/// Add a measurement to a point.
61
/**
62
 * Add measurement (in meas. system) with diagonal precision (inverse covariance) matrix.
63
 * ((up to) 2D: position, 4D: slope+position, 5D: curvature+slope+position)
64
 * \param [in] aProjection Projection from local to measurement system
65
66
 * \param [in] aResiduals Measurement residuals
 * \param [in] aPrecision Measurement precision (diagonal)
Claus Kleinwort's avatar
Claus Kleinwort committed
67
 * \param [in] minPrecision Minimal precision to accept measurement
68
69
 */
void GblPoint::addMeasurement(const TMatrixD &aProjection,
Claus Kleinwort's avatar
Claus Kleinwort committed
70
71
		const TVectorD &aResiduals, const TVectorD &aPrecision,
		double minPrecision) {
72
73
	measDim = aResiduals.GetNrows();
	unsigned int iOff = 5 - measDim;
74
	for (unsigned int i = 0; i < measDim; ++i) {
75
		measResiduals(iOff + i) = aResiduals[i];
Claus Kleinwort's avatar
Claus Kleinwort committed
76
77
		measPrecision(iOff + i) = (
				aPrecision[i] >= minPrecision ? aPrecision[i] : 0.);
78
		for (unsigned int j = 0; j < measDim; ++j) {
79
80
81
82
83
			measProjection(iOff + i, iOff + j) = aProjection(i, j);
		}
	}
}

84
/// Add a measurement to a point.
85
/**
86
 * Add measurement (in meas. system) with arbitrary precision (inverse covariance) matrix.
87
88
 * Will be diagonalized.
 * ((up to) 2D: position, 4D: slope+position, 5D: curvature+slope+position)
89
 * \param [in] aProjection Projection from local to measurement system
90
91
 * \param [in] aResiduals Measurement residuals
 * \param [in] aPrecision Measurement precision (matrix)
Claus Kleinwort's avatar
Claus Kleinwort committed
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
 * \param [in] minPrecision Minimal precision to accept measurement
 */
void GblPoint::addMeasurement(const TMatrixD &aProjection,
		const TVectorD &aResiduals, const TMatrixDSym &aPrecision,
		double minPrecision) {
	measDim = aResiduals.GetNrows();
	TMatrixDSymEigen measEigen(aPrecision);
	measTransformation.ResizeTo(measDim, measDim);
	measTransformation = measEigen.GetEigenVectors();
	measTransformation.T();
	transFlag = true;
	TVectorD transResiduals = measTransformation * aResiduals;
	TVectorD transPrecision = measEigen.GetEigenValues();
	TMatrixD transProjection = measTransformation * aProjection;
	unsigned int iOff = 5 - measDim;
	for (unsigned int i = 0; i < measDim; ++i) {
		measResiduals(iOff + i) = transResiduals[i];
		measPrecision(iOff + i) = (
				transPrecision[i] >= minPrecision ? transPrecision[i] : 0.);
		for (unsigned int j = 0; j < measDim; ++j) {
			measProjection(iOff + i, iOff + j) = transProjection(i, j);
		}
	}
}

117
/// Add a measurement to a point.
Claus Kleinwort's avatar
Claus Kleinwort committed
118
/**
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
 * Add measurement in local system with diagonal precision (inverse covariance) matrix.
 * ((up to) 2D: position, 4D: slope+position, 5D: curvature+slope+position)
 * \param [in] aResiduals Measurement residuals
 * \param [in] aPrecision Measurement precision (diagonal)
 * \param [in] minPrecision Minimal precision to accept measurement
 */
void GblPoint::addMeasurement(const TVectorD &aResiduals,
		const TVectorD &aPrecision, double minPrecision) {
	measDim = aResiduals.GetNrows();
	unsigned int iOff = 5 - measDim;
	for (unsigned int i = 0; i < measDim; ++i) {
		measResiduals(iOff + i) = aResiduals[i];
		measPrecision(iOff + i) = (
				aPrecision[i] >= minPrecision ? aPrecision[i] : 0.);
	}
	measProjection = ROOT::Math::SMatrixIdentity();
}

/// Add a measurement to a point.
/**
 * Add measurement in local system with arbitrary precision (inverse covariance) matrix.
Claus Kleinwort's avatar
Claus Kleinwort committed
140
141
142
143
144
 * Will be diagonalized.
 * ((up to) 2D: position, 4D: slope+position, 5D: curvature+slope+position)
 * \param [in] aResiduals Measurement residuals
 * \param [in] aPrecision Measurement precision (matrix)
 * \param [in] minPrecision Minimal precision to accept measurement
145
146
 */
void GblPoint::addMeasurement(const TVectorD &aResiduals,
Claus Kleinwort's avatar
Claus Kleinwort committed
147
		const TMatrixDSym &aPrecision, double minPrecision) {
148
149
	measDim = aResiduals.GetNrows();
	TMatrixDSymEigen measEigen(aPrecision);
150
	measTransformation.ResizeTo(measDim, measDim);
151
152
153
154
155
156
	measTransformation = measEigen.GetEigenVectors();
	measTransformation.T();
	transFlag = true;
	TVectorD transResiduals = measTransformation * aResiduals;
	TVectorD transPrecision = measEigen.GetEigenValues();
	unsigned int iOff = 5 - measDim;
157
	for (unsigned int i = 0; i < measDim; ++i) {
158
		measResiduals(iOff + i) = transResiduals[i];
Claus Kleinwort's avatar
Claus Kleinwort committed
159
160
		measPrecision(iOff + i) = (
				transPrecision[i] >= minPrecision ? transPrecision[i] : 0.);
161
		for (unsigned int j = 0; j < measDim; ++j) {
162
163
164
165
166
167
168
169
170
171
			measProjection(iOff + i, iOff + j) = measTransformation(i, j);
		}
	}
}

/// Check for measurement at a point.
/**
 * Get dimension of measurement (0 = none).
 * \return measurement dimension
 */
172
unsigned int GblPoint::hasMeasurement() const {
173
174
175
176
177
178
179
180
181
182
	return measDim;
}

/// Retrieve measurement of a point.
/**
 * \param [out] aProjection Projection from (diagonalized) measurement to local system
 * \param [out] aResiduals Measurement residuals
 * \param [out] aPrecision Measurement precision (diagonal)
 */
void GblPoint::getMeasurement(SMatrix55 &aProjection, SVector5 &aResiduals,
183
		SVector5 &aPrecision) const {
184
185
186
187
188
	aProjection = measProjection;
	aResiduals = measResiduals;
	aPrecision = measPrecision;
}

189
190
191
192
193
194
195
196
197
198
199
200
201
/// Get measurement transformation (from diagonalization).
/**
 * \param [out] aTransformation Transformation matrix
 */
void GblPoint::getMeasTransformation(TMatrixD &aTransformation) const {
	aTransformation.ResizeTo(measDim, measDim);
	if (transFlag) {
		aTransformation = measTransformation;
	} else {
		aTransformation.UnitMatrix();
	}
}

202
203
/// Add a (thin) scatterer to a point.
/**
204
 * Add scatterer with diagonal precision (inverse covariance) matrix.
205
206
 * Changes local track direction.
 *
207
208
209
210
211
212
213
214
215
216
 * \param [in] aResiduals Scatterer residuals
 * \param [in] aPrecision Scatterer precision (diagonal of inverse covariance matrix)
 */
void GblPoint::addScatterer(const TVectorD &aResiduals,
		const TVectorD &aPrecision) {
	scatFlag = true;
	scatResiduals(0) = aResiduals[0];
	scatResiduals(1) = aResiduals[1];
	scatPrecision(0) = aPrecision[0];
	scatPrecision(1) = aPrecision[1];
217
218
219
220
221
222
223
224
225
226
227
228
229
	scatTransformation = ROOT::Math::SMatrixIdentity();
}

/// Add a (thin) scatterer to a point.
/**
 * Add scatterer with arbitrary precision (inverse covariance) matrix.
 * Will be diagonalized. Changes local track direction.
 *
 * The precision matrix for the local slopes is defined by the
 * angular scattering error theta_0 and the scalar products c_1, c_2 of the
 * offset directions in the local frame with the track direction:
 *
 *            (1 - c_1*c_1 - c_2*c_2)   |  1 - c_1*c_1     - c_1*c_2  |
Claus Kleinwort's avatar
Claus Kleinwort committed
230
 *       P =  ----------------------- * |                             |
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
 *                theta_0*theta_0       |    - c_1*c_2   1 - c_2*c_2  |
 *
 * \param [in] aResiduals Scatterer residuals
 * \param [in] aPrecision Scatterer precision (matrix)
 */
void GblPoint::addScatterer(const TVectorD &aResiduals,
		const TMatrixDSym &aPrecision) {
	scatFlag = true;
	TMatrixDSymEigen scatEigen(aPrecision);
	TMatrixD aTransformation = scatEigen.GetEigenVectors();
	aTransformation.T();
	TVectorD transResiduals = aTransformation * aResiduals;
	TVectorD transPrecision = scatEigen.GetEigenValues();
	for (unsigned int i = 0; i < 2; ++i) {
		scatResiduals(i) = transResiduals[i];
		scatPrecision(i) = transPrecision[i];
		for (unsigned int j = 0; j < 2; ++j) {
			scatTransformation(i, j) = aTransformation(i, j);
		}
	}
251
252
253
}

/// Check for scatterer at a point.
254
bool GblPoint::hasScatterer() const {
255
256
257
258
259
	return scatFlag;
}

/// Retrieve scatterer of a point.
/**
260
 * \param [out] aTransformation Scatterer transformation from diagonalization
261
262
263
 * \param [out] aResiduals Scatterer residuals
 * \param [out] aPrecision Scatterer precision (diagonal)
 */
264
265
266
void GblPoint::getScatterer(SMatrix22 &aTransformation, SVector2 &aResiduals,
		SVector2 &aPrecision) const {
	aTransformation = scatTransformation;
267
268
269
270
	aResiduals = scatResiduals;
	aPrecision = scatPrecision;
}

271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
/// Get scatterer transformation (from diagonalization).
/**
 * \param [out] aTransformation Transformation matrix
 */
void GblPoint::getScatTransformation(TMatrixD &aTransformation) const {
	aTransformation.ResizeTo(2, 2);
	if (scatFlag) {
		for (unsigned int i = 0; i < 2; ++i) {
			for (unsigned int j = 0; j < 2; ++j) {
				aTransformation(i, j) = scatTransformation(i, j);
			}
		}
	} else {
		aTransformation.UnitMatrix();
	}
}

288
289
290
291
292
293
294
/// Add local derivatives to a point.
/**
 * Point needs to have a measurement.
 * \param [in] aDerivatives Local derivatives (matrix)
 */
void GblPoint::addLocals(const TMatrixD &aDerivatives) {
	if (measDim) {
295
		localDerivatives.ResizeTo(aDerivatives);
296
297
298
299
300
301
302
303
304
		if (transFlag) {
			localDerivatives = measTransformation * aDerivatives;
		} else {
			localDerivatives = aDerivatives;
		}
	}
}

/// Retrieve number of local derivatives from a point.
305
unsigned int GblPoint::getNumLocals() const {
306
307
308
309
	return localDerivatives.GetNcols();
}

/// Retrieve local derivatives from a point.
310
const TMatrixD& GblPoint::getLocalDerivatives() const {
311
312
313
314
315
316
317
318
319
320
321
322
323
	return localDerivatives;
}

/// Add global derivatives to a point.
/**
 * Point needs to have a measurement.
 * \param [in] aLabels Global derivatives labels
 * \param [in] aDerivatives Global derivatives (matrix)
 */
void GblPoint::addGlobals(const std::vector<int> &aLabels,
		const TMatrixD &aDerivatives) {
	if (measDim) {
		globalLabels = aLabels;
324
		globalDerivatives.ResizeTo(aDerivatives);
325
326
327
328
329
		if (transFlag) {
			globalDerivatives = measTransformation * aDerivatives;
		} else {
			globalDerivatives = aDerivatives;
		}
330

331
332
333
334
	}
}

/// Retrieve number of global derivatives from a point.
335
unsigned int GblPoint::getNumGlobals() const {
336
337
338
339
	return globalDerivatives.GetNcols();
}

/// Retrieve global derivatives labels from a point.
340
std::vector<int> GblPoint::getGlobalLabels() const {
341
342
343
344
	return globalLabels;
}

/// Retrieve global derivatives from a point.
345
const TMatrixD& GblPoint::getGlobalDerivatives() const {
346
347
348
	return globalDerivatives;
}

349
/// Define label of point (by GBLTrajectory constructor)
350
351
352
353
354
355
356
357
/**
 * \param [in] aLabel Label identifying point
 */
void GblPoint::setLabel(unsigned int aLabel) {
	theLabel = aLabel;
}

/// Retrieve label of point
358
unsigned int GblPoint::getLabel() const {
359
360
361
	return theLabel;
}

362
/// Define offset for point (by GBLTrajectory constructor)
363
364
365
366
367
368
369
370
/**
 * \param [in] anOffset Offset number
 */
void GblPoint::setOffset(int anOffset) {
	theOffset = anOffset;
}

/// Retrieve offset for point
371
int GblPoint::getOffset() const {
372
373
374
375
	return theOffset;
}

/// Retrieve point-to-(previous)point jacobian
376
const SMatrix55& GblPoint::getP2pJacobian() const {
377
378
379
	return p2pJacobian;
}

380
/// Define jacobian to previous scatterer (by GBLTrajectory constructor)
381
382
383
/**
 * \param [in] aJac Jacobian
 */
384
void GblPoint::addPrevJacobian(const SMatrix55 &aJac) {
385
386
	int ifail = 0;
// to optimize: need only two last rows of inverse
387
388
389
390
391
392
393
394
//	prevJacobian = aJac.InverseFast(ifail);
//  block matrix algebra
	SMatrix23 CA = aJac.Sub<SMatrix23>(3, 0)
			* aJac.Sub<SMatrix33>(0, 0).InverseFast(ifail); // C*A^-1
	SMatrix22 DCAB = aJac.Sub<SMatrix22>(3, 3) - CA * aJac.Sub<SMatrix32>(0, 3); // D - C*A^-1 *B
	DCAB.InvertFast();
	prevJacobian.Place_at(DCAB, 3, 3);
	prevJacobian.Place_at(-DCAB * CA, 3, 0);
395
396
}

397
/// Define jacobian to next scatterer (by GBLTrajectory constructor)
398
399
400
/**
 * \param [in] aJac Jacobian
 */
401
void GblPoint::addNextJacobian(const SMatrix55 &aJac) {
402
403
404
405
406
407
408
409
410
411
412
	nextJacobian = aJac;
}

/// Retrieve derivatives of local track model
/**
 * Linearized track model: F_u(q/p,u',u) = J*u + S*u' + d*q/p,
 * W is inverse of S, negated for backward propagation.
 * \param [in] aDirection Propagation direction (>0 forward, else backward)
 * \param [out] matW W
 * \param [out] matWJ W*J
 * \param [out] vecWd W*d
413
 * \exception std::overflow_error : matrix S is singular.
414
415
 */
void GblPoint::getDerivatives(int aDirection, SMatrix22 &matW, SMatrix22 &matWJ,
416
		SVector2 &vecWd) const {
417

418
419
	SMatrix22 matJ;
	SVector2 vecd;
420
	if (aDirection < 1) {
421
		matJ = prevJacobian.Sub<SMatrix22>(3, 3);
422
		matW = -prevJacobian.Sub<SMatrix22>(3, 1);
423
		vecd = prevJacobian.SubCol<SVector2>(0, 3);
424
	} else {
425
		matJ = nextJacobian.Sub<SMatrix22>(3, 3);
426
		matW = nextJacobian.Sub<SMatrix22>(3, 1);
427
		vecd = nextJacobian.SubCol<SVector2>(0, 3);
428
	}
Claus Kleinwort's avatar
Claus Kleinwort committed
429

430
431
	if (!matW.InvertFast()) {
		std::cout << " GblPoint::getDerivatives failed to invert matrix: "
432
				<< matW << std::endl;
433
434
435
		std::cout
				<< " Possible reason for singular matrix: multiple GblPoints at same arc-length"
				<< std::endl;
436
437
		throw std::overflow_error("Singular matrix inversion exception");
	}
438
439
	matWJ = matW * matJ;
	vecWd = matW * vecd;
Claus Kleinwort's avatar
Claus Kleinwort committed
440

441
}
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
476
477
478
479
480
481
482
483
484
485
486
487
488
489
490
491
492
493
494
495
496
497
498
499
500
501
502
503
504
505
506
507
508
509
510

/// Print GblPoint
/**
 * \param [in] level print level (0: minimum, >0: more)
 */
void GblPoint::printPoint(unsigned int level) const {
	std::cout << " GblPoint";
	if (theLabel) {
		std::cout << ", label " << theLabel;
		if (theOffset >= 0) {
			std::cout << ", offset " << theOffset;
		}
	}
	if (measDim) {
		std::cout << ", " << measDim << " measurements";
	}
	if (scatFlag) {
		std::cout << ", scatterer";
	}
	if (transFlag) {
		std::cout << ", diagonalized";
	}
	if (localDerivatives.GetNcols()) {
		std::cout << ", " << localDerivatives.GetNcols()
				<< " local derivatives";
	}
	if (globalDerivatives.GetNcols()) {
		std::cout << ", " << globalDerivatives.GetNcols()
				<< " global derivatives";
	}
	std::cout << std::endl;
	if (level > 0) {
		if (measDim) {
			std::cout << "  Measurement" << std::endl;
			std::cout << "   Projection: " << std::endl << measProjection
					<< std::endl;
			std::cout << "   Residuals: " << measResiduals << std::endl;
			std::cout << "   Precision: " << measPrecision << std::endl;
		}
		if (scatFlag) {
			std::cout << "  Scatterer" << std::endl;
			std::cout << "   Residuals: " << scatResiduals << std::endl;
			std::cout << "   Precision: " << scatPrecision << std::endl;
		}
		if (localDerivatives.GetNcols()) {
			std::cout << "  Local Derivatives:" << std::endl;
			localDerivatives.Print();
		}
		if (globalDerivatives.GetNcols()) {
			std::cout << "  Global Labels:";
			for (unsigned int i = 0; i < globalLabels.size(); ++i) {
				std::cout << " " << globalLabels[i];
			}
			std::cout << std::endl;
			std::cout << "  Global Derivatives:" << std::endl;
			globalDerivatives.Print();
		}
		std::cout << "  Jacobian " << std::endl;
		std::cout << "   Point-to-point " << std::endl << p2pJacobian
				<< std::endl;
		if (theLabel) {
			std::cout << "   To previous offset " << std::endl << prevJacobian
					<< std::endl;
			std::cout << "   To next offset " << std::endl << nextJacobian
					<< std::endl;
		}
	}
}

511
}