LArSoft  v09_90_00
Liquid Argon Software toolkit - https://larsoft.org/
KETrack.cxx
Go to the documentation of this file.
1 
12 #include "cetlib_except/exception.h"
13 #include <cmath>
14 
15 namespace trkf {
16 
19 
26  KETrack::KETrack(const std::shared_ptr<const Surface>& psurf) : KTrack(psurf) {}
27 
38  KETrack::KETrack(const std::shared_ptr<const Surface>& psurf,
39  const TrackVector& vec,
40  const TrackError& err,
42  int pdg)
43  : KTrack(psurf, vec, dir, pdg), fErr(err)
44  {}
45 
53  KETrack::KETrack(const KTrack& trk, const TrackError& err) : KTrack(trk), fErr(err) {}
54 
57 
66  double KETrack::PointingError() const
67  {
68  if (!isValid())
69  throw cet::exception("KETrack") << "Pointing error requested for invalid track.\n";
70  return getSurface()->PointingError(getVector(), fErr);
71  }
72 
87  std::optional<double> KETrack::combineTrack(const KETrack& tre)
88  {
89  // Make sure that the two track surfaces are the same.
90  // Throw an exception if they are not.
91 
92  if (!getSurface()->isEqual(*tre.getSurface()))
93  throw cet::exception("KETrack") << "Track combination surfaces are not the same.\n";
94 
95  // Default result is failure.
96 
97  std::optional<double> result{std::nullopt};
98 
99  // We will use asymmetric versions of the updating formulas, such
100  // that the result is calculated as a perturbation on the
101  // better-measured track. We define the better measured track as
102  // the one with the smaller error matrix trace.
103 
104  // Extract the two state vectors and error matrices as pointers.
105 
106  const TrackVector* vec1 = &getVector();
107  const TrackError* err1 = &getError();
108  const TrackVector* vec2 = &tre.getVector();
109  const TrackError* err2 = &tre.getError();
110 
111  // Calculate the traces of the error matrices.
112 
113  double tr1 = 0;
114  for (unsigned int i = 0; i < err1->size1(); ++i)
115  tr1 += (*err1)(i, i);
116 
117  double tr2 = 0;
118  for (unsigned int i = 0; i < err2->size1(); ++i)
119  tr2 += (*err2)(i, i);
120 
121  // Define vec1, err1 as belong to the better measured track.
122  // Swap if necessary.
123 
124  if (tr1 > tr2) {
125  const TrackVector* tvec = vec1;
126  vec1 = vec2;
127  vec2 = tvec;
128  const TrackError* terr = err1;
129  err1 = err2;
130  err2 = terr;
131  }
132 
133  // Calculate the difference vector and difference error matrix.
134 
135  TrackVector dvec = *vec1 - *vec2;
136  TrackError derr = *err1 + *err2;
137 
138  // Invert the difference error matrix.
139  // This is the only place where a detectable failure can occur.
140 
141  bool ok = syminvert(derr);
142  if (ok) {
143 
144  // Calculate updated state vector.
145  // vec1 = vec1 - err1 * derr * dvec
146 
147  TrackVector tvec1 = prod(derr, dvec);
148  TrackVector tvec2 = prod(*err1, tvec1);
149  TrackVector tvec3 = *vec1 - tvec2;
150  setVector(tvec3);
151 
152  // Calculate updated error matrix.
153  // err1 = err1 - err1 * derr * err1
154 
155  TrackMatrix terr1 = prod(derr, *err1);
156  TrackMatrix terr2 = prod(*err1, terr1);
157  TrackError terr2s = ublas::symmetric_adaptor<TrackMatrix>(terr2);
158  TrackError terr3 = *err1 - terr2s;
159  setError(terr3);
160 
161  // Calculate chisquare.
162  // chisq = dvec^T * derr * dvec
163 
164  TrackVector dvec1 = prod(derr, dvec);
165  double chisq = inner_prod(dvec, dvec1);
166  result = std::make_optional(chisq);
167  }
168 
169  // Final validity check.
170 
171  if (!isValid()) result = std::nullopt;
172 
173  // Done.
174 
175  return result;
176  }
177 
179  std::ostream& KETrack::Print(std::ostream& out, bool doTitle) const
180  {
181  if (doTitle) out << "KETrack:\n";
182 
183  // Print base class.
184 
185  KTrack::Print(out, false);
186 
187  // Print diagonal errors.
188 
189  out << " Diagonal errors:\n"
190  << " [";
191  for (unsigned int i = 0; i < fErr.size1(); ++i) {
192  if (i != 0) out << ", ";
193  double err = fErr(i, i);
194  err = (err >= 0. ? std::sqrt(err) : -std::sqrt(-err));
195  out << err;
196  }
197  out << "]\n";
198 
199  // Print correlations.
200 
201  out << " Correlation matrix:";
202  for (unsigned int i = 0; i < fErr.size1(); ++i) {
203  if (i == 0)
204  out << "\n [";
205  else
206  out << "\n ";
207  for (unsigned int j = 0; j <= i; ++j) {
208  if (j != 0) out << ", ";
209  if (i == j)
210  out << 1.;
211  else {
212  double eiijj = fErr(i, i) * fErr(j, j);
213  double eij = fErr(i, j);
214  if (eiijj != 0.)
215  eij /= std::sqrt(std::abs(eiijj));
216  else
217  eij = 0.;
218  out << eij;
219  }
220  }
221  }
222  out << "]\n";
223  out << " Pointing error = " << PointingError() << "\n";
224  return out;
225  }
226 
227 } // end namespace trkf
const TrackError & getError() const
Track error matrix.
Definition: KETrack.h:52
TrackDirection
Track direction enum.
Definition: Surface.h:54
const std::shared_ptr< const Surface > & getSurface() const
Surface.
Definition: KTrack.h:53
double PointingError() const
Pointing error (radians).
Definition: KETrack.cxx:66
KSymMatrix< 5 >::type TrackError
Track error matrix, dimension 5x5.
void setVector(const TrackVector &vec)
Set state vector.
Definition: KTrack.h:65
constexpr auto abs(T v)
Returns the absolute value of the argument.
KETrack()
Default constructor.
Definition: KETrack.cxx:18
void setError(const TrackError &err)
Set error matrix.
Definition: KETrack.h:58
KMatrix< 5, 5 >::type TrackMatrix
General 5x5 matrix.
virtual ~KETrack()
Destructor.
Definition: KETrack.cxx:56
bool syminvert(ublas::symmetric_matrix< T, TRI, L, A > &m)
std::optional< double > combineTrack(const KETrack &tre)
Combine two tracks.
Definition: KETrack.cxx:87
KVector< 5 >::type TrackVector
Track state vector, dimension 5.
virtual std::ostream & Print(std::ostream &out, bool doTitle=true) const
Printout.
Definition: KTrack.cxx:210
const TrackVector & getVector() const
Track state vector.
Definition: KTrack.h:54
TDirectory * dir
Definition: macro.C:5
TrackError fErr
Track error matrix.
Definition: KETrack.h:69
virtual std::ostream & Print(std::ostream &out, bool doTitle=true) const
Printout.
Definition: KETrack.cxx:179
Basic Kalman filter track class, with error.
cet::coded_exception< error, detail::translate > exception
Definition: exception.h:33
bool isValid() const
Test if track is valid.
Definition: KTrack.cxx:81