LArSoft  v09_90_00
Liquid Argon Software toolkit - https://larsoft.org/
KHit.h
Go to the documentation of this file.
1 
64 #ifndef KHIT_H
65 #define KHIT_H
66 
67 #include "cetlib_except/exception.h"
70 
71 namespace trkf {
72 
73  template <int N>
74  class KHit : public KHitBase {
75  public:
77  KHit();
78 
80  KHit(const std::shared_ptr<const Surface>& psurf);
81 
83  KHit(const std::shared_ptr<const Surface>& psurf,
84  const typename KVector<N>::type& mvec,
85  const typename KSymMatrix<N>::type& merr);
86 
88  virtual ~KHit();
89 
90  // Modifiers.
91 
93  void setMeasVector(const typename KVector<N>::type& mvec) { fMvec = mvec; }
94 
96  void setMeasError(const typename KSymMatrix<N>::type& merr) { fMerr = merr; }
97 
98  // Accessors.
99 
101  const typename KVector<N>::type& getMeasVector() const { return fMvec; }
102 
104  const typename KSymMatrix<N>::type& getMeasError() const { return fMerr; }
105 
107  const typename KVector<N>::type& getPredVector() const { return fPvec; }
108 
110  const typename KSymMatrix<N>::type& getPredError() const { return fPerr; }
111 
113  const typename KVector<N>::type& getResVector() const { return fRvec; }
114 
116  const typename KSymMatrix<N>::type& getResError() const { return fRerr; }
117 
119  const typename KSymMatrix<N>::type& getResInvError() const { return fRinv; }
120 
122  const typename KHMatrix<N>::type& getH() const { return fH; }
123 
125  double getChisq() const { return fChisq; }
126 
127  // Overrides.
128  // Implementation of overrides is found at the bottom of this header.
129 
131  bool predict(const KETrack& tre, const Propagator& prop, const KTrack* ref = 0) const;
132 
134  void update(KETrack& tre) const;
135 
136  // Pure virtual methods.
137 
139  virtual bool subpredict(const KETrack& tre,
140  typename KVector<N>::type& pvec,
141  typename KSymMatrix<N>::type& perr,
142  typename KHMatrix<N>::type& hmatrix) const = 0;
143 
145  virtual std::ostream& Print(std::ostream& out, bool doTitle = true) const;
146 
147  private:
148  // Attributes.
149 
152  mutable typename KVector<N>::type fPvec;
153  mutable typename KSymMatrix<N>::type fPerr;
154  mutable typename KVector<N>::type fRvec;
155  mutable typename KSymMatrix<N>::type fRerr;
156  mutable typename KSymMatrix<N>::type fRinv;
157  mutable typename KHMatrix<N>::type fH;
158  mutable double fChisq;
159  };
160 
161  // Method implementations.
162 
164  template <int N>
166  {}
167 
174  template <int N>
175  KHit<N>::KHit(const std::shared_ptr<const Surface>& psurf) : KHitBase(psurf), fChisq(0.)
176  {}
177 
186  template <int N>
187  KHit<N>::KHit(const std::shared_ptr<const Surface>& psurf,
188  const typename KVector<N>::type& mvec,
189  const typename KSymMatrix<N>::type& merr)
190  : KHitBase(psurf), fMvec(mvec), fMerr(merr), fChisq(0.)
191  {}
192 
194  template <int N>
196  {}
197 
206  template <int N>
207  bool KHit<N>::predict(const KETrack& tre, const Propagator& prop, const KTrack* ref) const
208  {
209  // Update the prediction surface to be the track surface.
210 
211  fPredSurf = tre.getSurface();
212  fPredDist = 0.;
213 
214  // Default result.
215 
216  bool ok = false;
217 
218  // Update prediction vector, error matrox, and H-matrix.
219 
220  // First test whether the prediction surface matches the
221  // measurement surface.
222 
223  if (getMeasSurface()->isEqual(*fPredSurf)) {
224 
225  // Prediction and measurement surfaces agree.
226  //Just call subpredict method (don't do propagation).
227 
228  ok = subpredict(tre, fPvec, fPerr, fH);
229  }
230  else {
231 
232  // Track surface does not match the prediction surface, so an
233  // internal propagation will be required. Throw an exception if
234  // no propagator was provided.
235 
236  // First make a copy of the original KETrack.
237 
238  KETrack treprop(tre);
239 
240  // Also make a copy of the reference track (if specified).
241 
242  KTrack refprop;
243  KTrack* prefprop = 0;
244  if (ref != 0) {
245  refprop = *ref;
246  prefprop = &refprop;
247  }
248 
249  // Make a no-noise, no-dE/dx propagation to the measurement
250  // surface. But do calculate the propagation matrix, which we
251  // will use to update the H-matrix calculated in the derived
252  // class.
253 
254  TrackMatrix prop_matrix;
255  std::optional<double> dist = prop.err_prop(
256  treprop, getMeasSurface(), Propagator::UNKNOWN, false, prefprop, &prop_matrix);
257  if (dist) {
258 
259  // Update prediction distance.
260 
261  fPredDist = *dist;
262 
263  // Now we are ready to calculate the prediction on the
264  // measurement surface.
265 
266  typename KHMatrix<N>::type hmatrix;
267  ok = subpredict(treprop, fPvec, fPerr, hmatrix);
268  if (ok) {
269 
270  // Use the propagation matrix to transform the H-matrix back
271  // to the prediction surface.
272 
273  fH = prod(hmatrix, prop_matrix);
274  }
275  }
276  }
277  if (ok) {
278 
279  // Update residual
280 
281  fRvec = fMvec - fPvec;
282  fRerr = fMerr + fPerr;
283  fRinv = fRerr;
284  ok = syminvert(fRinv);
285  if (ok) {
286 
287  // Calculate incremental chisquare.
288 
289  // (workaround: if we use the copy constructor, gcc emits a spurious warning)
290  // typename KVector<N>::type rtemp = prod(fRinv, fRvec);
291  fChisq = inner_prod(fRvec, prod(fRinv, fRvec));
292  }
293  }
294 
295  // If a problem occured at any step, clear the prediction surface pointer.
296 
297  if (!ok) {
298  fPredSurf.reset();
299  fPredDist = 0.;
300  }
301 
302  // Done.
303 
304  return ok;
305  }
306 
313  template <int N>
314  void KHit<N>::update(KETrack& tre) const
315  {
316  // Make sure that the track surface and the prediction surface are the same.
317  // Throw an exception if they are not.
318 
319  if (!getPredSurface()->isEqual(*tre.getSurface()))
320  throw cet::exception("KHit") << "Track surface not the same as prediction surface.\n";
321 
322  const TrackVector& tvec = tre.getVector();
323  const TrackError& terr = tre.getError();
324  TrackVector::size_type size = tvec.size();
325 
326  // Calculate gain matrix.
327 
328  typename KGMatrix<N>::type temp(size, N);
329  typename KGMatrix<N>::type gain(size, N);
330  temp = prod(trans(fH), fRinv);
331  gain = prod(terr, temp);
332 
333  // Calculate updated track state.
334 
335  TrackVector newvec = tre.getVector() + prod(gain, fRvec);
336 
337  // Calculate updated error matrix.
338 
339  TrackMatrix fact = ublas::identity_matrix<TrackVector::value_type>(size);
340  fact -= prod(gain, fH);
341  TrackMatrix errtemp1 = prod(terr, trans(fact));
342  TrackMatrix errtemp2 = prod(fact, errtemp1);
343  TrackError errtemp2s = ublas::symmetric_adaptor<TrackMatrix>(errtemp2);
344  typename KHMatrix<N>::type errtemp3 = prod(fMerr, trans(gain));
345  TrackMatrix errtemp4 = prod(gain, errtemp3);
346  TrackError errtemp4s = ublas::symmetric_adaptor<TrackMatrix>(errtemp4);
347  TrackError newerr = errtemp2s + errtemp4s;
348 
349  // Update track.
350 
351  tre.setVector(newvec);
352  tre.setError(newerr);
353  }
354 
356  template <int N>
357  std::ostream& KHit<N>::Print(std::ostream& out, bool doTitle) const
358  {
359  if (doTitle) out << "KHit<" << N << ">:\n";
360 
361  // Print base class.
362 
363  KHitBase::Print(out, false);
364 
365  // Print measurement vector.
366 
367  out << " Measurement vector:\n"
368  << " [";
369  for (unsigned int i = 0; i < fMvec.size(); ++i) {
370  if (i != 0) out << ", ";
371  out << fMvec(i);
372  }
373  out << "]\n";
374 
375  // Print diagonal measurement errors.
376 
377  out << " Diagonal measurement errors:\n"
378  << " [";
379  for (unsigned int i = 0; i < fMerr.size1(); ++i) {
380  if (i != 0) out << ", ";
381  double err = fMerr(i, i);
382  err = (err >= 0. ? std::sqrt(err) : -std::sqrt(-err));
383  out << err;
384  }
385  out << "]\n";
386 
387  // Print measurement correlations.
388 
389  if (fMerr.size1() > 1) {
390  out << " Measurement correlation matrix:";
391  for (unsigned int i = 0; i < fMerr.size1(); ++i) {
392  if (i == 0)
393  out << "\n [";
394  else
395  out << "\n ";
396  for (unsigned int j = 0; j <= i; ++j) {
397  if (j != 0) out << ", ";
398  if (i == j)
399  out << 1.;
400  else {
401  double eiijj = fMerr(i, i) * fMerr(j, j);
402  double eij = fMerr(i, j);
403  if (eiijj != 0.)
404  eij /= std::sqrt(std::abs(eiijj));
405  else
406  eij = 0.;
407  out << eij;
408  }
409  }
410  }
411  out << "]\n";
412  }
413 
414  // Print prediction vector.
415 
416  out << " Prediction vector:\n"
417  << " [";
418  for (unsigned int i = 0; i < fPvec.size(); ++i) {
419  if (i != 0) out << ", ";
420  out << fPvec(i);
421  }
422  out << "]\n";
423 
424  // Print diagonal prediction errors.
425 
426  out << " Diagonal prediction errors:\n"
427  << " [";
428  for (unsigned int i = 0; i < fPerr.size1(); ++i) {
429  if (i != 0) out << ", ";
430  double err = fPerr(i, i);
431  err = (err >= 0. ? std::sqrt(err) : -std::sqrt(-err));
432  out << err;
433  }
434  out << "]\n";
435 
436  // Print prediction correlations.
437 
438  if (fPerr.size1() > 1) {
439  out << " Prediction correlation matrix:";
440  for (unsigned int i = 0; i < fPerr.size1(); ++i) {
441  if (i == 0)
442  out << "\n [";
443  else
444  out << "\n ";
445  for (unsigned int j = 0; j <= i; ++j) {
446  if (j != 0) out << ", ";
447  if (i == j)
448  out << 1.;
449  else {
450  double eiijj = fPerr(i, i) * fPerr(j, j);
451  double eij = fPerr(i, j);
452  if (eiijj != 0.)
453  eij /= std::sqrt(std::abs(eiijj));
454  else
455  eij = 0.;
456  out << eij;
457  }
458  }
459  }
460  out << "]\n";
461  }
462 
463  // Print residual vector.
464 
465  out << " Residual vector:\n"
466  << " [";
467  for (unsigned int i = 0; i < fRvec.size(); ++i) {
468  if (i != 0) out << ", ";
469  out << fRvec(i);
470  }
471  out << "]\n";
472 
473  // Print diagonal residual errors.
474 
475  out << " Diagonal residual errors:\n"
476  << " [";
477  for (unsigned int i = 0; i < fRerr.size1(); ++i) {
478  if (i != 0) out << ", ";
479  double err = fRerr(i, i);
480  err = (err >= 0. ? std::sqrt(err) : -std::sqrt(-err));
481  out << err;
482  }
483  out << "]\n";
484 
485  // Print residual correlations.
486 
487  if (fRerr.size1() > 1) {
488  out << " Residual correlation matrix:";
489  for (unsigned int i = 0; i < fRerr.size1(); ++i) {
490  if (i == 0)
491  out << "\n [";
492  else
493  out << "\n ";
494  for (unsigned int j = 0; j <= i; ++j) {
495  if (j != 0) out << ", ";
496  if (i == j)
497  out << 1.;
498  else {
499  double eiijj = fRerr(i, i) * fRerr(j, j);
500  double eij = fRerr(i, j);
501  if (eiijj != 0.)
502  eij /= std::sqrt(std::abs(eiijj));
503  else
504  eij = 0.;
505  out << eij;
506  }
507  }
508  }
509  out << "]\n";
510  }
511 
512  // Print incremental chisquare.
513 
514  out << " Incremental chisquare = " << fChisq << "\n";
515 
516  // Done.
517 
518  return out;
519  }
520 }
521 
522 #endif
bool predict(const KETrack &tre, const Propagator &prop, const KTrack *ref=0) const
Prediction method (return false if fail).
Definition: KHit.h:207
const TrackError & getError() const
Track error matrix.
Definition: KETrack.h:52
virtual bool subpredict(const KETrack &tre, typename KVector< N >::type &pvec, typename KSymMatrix< N >::type &perr, typename KHMatrix< N >::type &hmatrix) const =0
Calculate prediction function (return via arguments).
const std::shared_ptr< const Surface > & getSurface() const
Surface.
Definition: KTrack.h:53
void update(KETrack &tre) const
Update track method.
Definition: KHit.h:314
KSymMatrix< 5 >::type TrackError
Track error matrix, dimension 5x5.
void setVector(const TrackVector &vec)
Set state vector.
Definition: KTrack.h:65
const KSymMatrix< N >::type & getMeasError() const
Measurement error matrix.
Definition: KHit.h:104
constexpr auto abs(T v)
Returns the absolute value of the argument.
virtual std::ostream & Print(std::ostream &out, bool doTitle=true) const
Printout.
Definition: KHit.h:357
const KHMatrix< N >::type & getH() const
Kalman H-matrix.
Definition: KHit.h:122
const KVector< N >::type & getResVector() const
Residual vector.
Definition: KHit.h:113
virtual std::ostream & Print(std::ostream &out, bool doTitle=true) const
Printout.
Definition: KHitBase.cxx:31
void setError(const TrackError &err)
Set error matrix.
Definition: KETrack.h:58
const KVector< N >::type & getPredVector() const
Prediction vector.
Definition: KHit.h:107
const KSymMatrix< N >::type & getPredError() const
Prediction matrix.
Definition: KHit.h:110
ublas::symmetric_matrix< double, ublas::lower, ublas::row_major, ublas::bounded_array< double, N *(N+1)/2 > > type
KVector< N >::type fPvec
Prediction vector.
Definition: KHit.h:152
KHit()
Default constructor.
Definition: KHit.h:165
decltype(auto) constexpr size(T &&obj)
ADL-aware version of std::size.
Definition: StdUtils.h:101
KMatrix< 5, 5 >::type TrackMatrix
General 5x5 matrix.
KVector< N >::type fMvec
Measurement vector.
Definition: KHit.h:150
KVector< N >::type fRvec
Residual vector.
Definition: KHit.h:154
void setMeasVector(const typename KVector< N >::type &mvec)
Set measurement vector.
Definition: KHit.h:93
std::shared_ptr< const Surface > fPredSurf
Prediction surface.
Definition: KHitBase.h:117
const KSymMatrix< N >::type & getResError() const
Residual error matrix.
Definition: KHit.h:116
KMatrix< 5, N >::type type
ublas::vector< double, ublas::bounded_array< double, N > > type
bool syminvert(ublas::symmetric_matrix< T, TRI, L, A > &m)
KHMatrix< N >::type fH
Kalman H-matrix.
Definition: KHit.h:157
const KSymMatrix< N >::type & getResInvError() const
Residual inv. error matrix.
Definition: KHit.h:119
double getChisq() const
Incremental chisquare.
Definition: KHit.h:125
std::optional< double > err_prop(KETrack &tre, const std::shared_ptr< const Surface > &psurf, PropDirection dir, bool doDedx, KTrack *ref=0, TrackMatrix *prop_matrix=0) const
Propagate with error, but without noise.
Definition: Propagator.cxx:343
KSymMatrix< N >::type fRinv
Residual inverse error matrix.
Definition: KHit.h:156
Base class for Kalman filter track propagator.
KVector< 5 >::type TrackVector
Track state vector, dimension 5.
void setMeasError(const typename KSymMatrix< N >::type &merr)
Set measurement error.
Definition: KHit.h:96
double fPredDist
Prediction distance.
Definition: KHitBase.h:118
KMatrix< N, 5 >::type type
const TrackVector & getVector() const
Track state vector.
Definition: KTrack.h:54
KSymMatrix< N >::type fRerr
Residual error matrix.
Definition: KHit.h:155
const KVector< N >::type & getMeasVector() const
Measurement vector.
Definition: KHit.h:101
constexpr double dist(const TReal *x, const TReal *y, const unsigned int dimension)
KSymMatrix< N >::type fPerr
Prediction error matrix.
Definition: KHit.h:153
virtual ~KHit()
Destructor.
Definition: KHit.h:195
KSymMatrix< N >::type fMerr
Measurement error matrix.
Definition: KHit.h:151
Base class for Kalman filter measurement.
const std::shared_ptr< const Surface > & getMeasSurface() const
Measurement surface.
Definition: KHitBase.h:82
cet::coded_exception< error, detail::translate > exception
Definition: exception.h:33
double fChisq
Incremental chisquare.
Definition: KHit.h:158
const std::shared_ptr< const Surface > & getPredSurface() const
Predition surface.
Definition: KHitBase.h:76