00001
00002
00003
00004
00005
00006
00007
00008
00010
00011 #include "CandFitTrackSR/KalmanPlaneSR.h"
00012
00013
00014 #include <iostream>
00015 #include <string>
00016
00017 #include "Algorithm/AlgConfig.h"
00018 #include "TVector3.h"
00019 #include "CandTrackSR/TrackClusterSR.h"
00020 #include "Conventions/PlaneView.h"
00021 #include "Conventions/Mphysical.h"
00022 #include "Conventions/Munits.h"
00023 #include "MessageService/MsgFormat.h"
00024 #include "MessageService/MsgService.h"
00025 #include "RecoBase/CandStripHandle.h"
00026 #include "UgliGeometry/UgliGeomHandle.h"
00027 #include "Validity/VldContext.h"
00028
00029 ClassImp(KalmanPlaneSR)
00030
00031
00032 CVSID("$Id: KalmanPlaneSR.cxx,v 1.28 2005/03/24 19:50:36 brebel Exp $");
00033
00034
00035 KalmanPlaneSR::KalmanPlaneSR() :
00036 fTrackCluster(0)
00037 {
00038
00039 InitKalman();
00040
00041 }
00042
00043
00044 KalmanPlaneSR::KalmanPlaneSR(TrackClusterSR *tcluster)
00045 {
00046 fTrackCluster = tcluster;
00047 InitKalman();
00048 }
00049
00050
00051 KalmanPlaneSR::KalmanPlaneSR(KalmanPlaneSR *kp)
00052 {
00053 fTrackCluster = kp->GetTrackCluster();
00054 Init(kp);
00055 }
00056
00057
00058
00059 KalmanPlaneSR::~KalmanPlaneSR()
00060 {
00061 }
00062
00063
00064 void KalmanPlaneSR::InitKalman()
00065 {
00066 for(Int_t i=0; i<2; ++i){
00067 fPreState[i].ResizeTo(5);
00068 fFilState[i].ResizeTo(5);
00069 fGain[i].ResizeTo(5);
00070 fPropagator[i].ResizeTo(5,5);
00071 fPreCovariance[i].ResizeTo(5,5);
00072 fFilCovariance[i].ResizeTo(5,5);
00073 fNoise[i].ResizeTo(5,5);
00074 for(Int_t j=0; j<5; ++j){
00075 fPropagator[i](j,j) = 1.;
00076 fPreCovariance[i](j,j) = 1.;
00077 fFilCovariance[i](j,j) = 1.;
00078 }
00079 fPredictPlane[i] = -999;
00080 fPreChi2[i] = 0.;
00081 fFilChi2[i] = 0.;
00082 }
00083 fZDir = 1;
00084 fRange = 0.;
00085 }
00086
00087
00088 void KalmanPlaneSR::Init(KalmanPlaneSR *kp)
00089 {
00090 InitKalman();
00091
00092 for(int i=0; i<2; ++i){
00093 fPreState[i] = kp->fPreState[i];
00094 fFilState[i] = kp->fFilState[i];
00095 fGain[i] = kp->fGain[i];
00096 fPropagator[i] = kp->fPropagator[i];
00097 fPreCovariance[i] = kp->fPreCovariance[i];
00098 fFilCovariance[i] = kp->fFilCovariance[i];
00099 fNoise[i] = kp->fNoise[i];
00100 fPreChi2[i] = kp->fPreChi2[i];
00101 fFilChi2[i] = kp->fFilChi2[i];
00102 fPredictPlane[i] = kp->fPredictPlane[i];
00103 }
00104 fZDir = kp->GetZDir();
00105 fRange = kp->GetRange();
00106
00107 }
00108
00109
00110 const VldContext *KalmanPlaneSR::GetVldContext()
00111 {
00112 if(!dynamic_cast<CandStripHandle*>
00113 (fTrackCluster->GetStripList()->First())->GetVldContext())
00114 MSG("FitTrackSR", Msg::kWarning) << "No VldContext for KalmanPlaneSR" << endl;
00115 return dynamic_cast<CandStripHandle*>(fTrackCluster->GetStripList()->First())->GetVldContext();
00116 }
00117
00118
00119 Int_t KalmanPlaneSR::GetPredictPlane(Int_t idir) const
00120 {
00121 return fPredictPlane[idir];
00122 }
00123
00124
00125 void KalmanPlaneSR::SetPredictPlane(Int_t plane, Int_t idir)
00126 {
00127 fPredictPlane[idir] = plane;
00128 return;
00129 }
00130
00131
00132 Double_t KalmanPlaneSR::GetPreStateValue(Int_t stateIndex, Int_t idir) const
00133 {
00134 return fPreState[idir](stateIndex);
00135 }
00136
00137
00138 void KalmanPlaneSR::SetPreStateValue(Int_t stateIndex, Int_t idir,
00139 Double_t stateValue)
00140 {
00141 fPreState[idir](stateIndex) = stateValue;
00142 return;
00143 }
00144
00145
00146 Double_t KalmanPlaneSR::GetFilStateValue(Int_t stateIndex, Int_t idir) const
00147 {
00148 return fFilState[idir](stateIndex);
00149 }
00150
00151
00152 void KalmanPlaneSR::SetFilStateValue(Int_t stateIndex, Int_t idir,
00153 Double_t stateValue)
00154 {
00155 fFilState[idir](stateIndex) = stateValue;
00156 return;
00157 }
00158
00159
00160 Double_t KalmanPlaneSR::GetGainValue(Int_t gainIndex, Int_t idir) const
00161 {
00162 return fGain[idir](gainIndex);
00163 }
00164
00165
00166 void KalmanPlaneSR::SetGainValue(Int_t gainIndex, Int_t idir,
00167 Double_t gainValue)
00168 {
00169 fGain[idir](gainIndex) = gainValue;
00170 return;
00171 }
00172
00173
00174 Double_t KalmanPlaneSR::GetPropagatorMatrixValue(Int_t idir, Int_t row,
00175 Int_t column) const
00176 {
00177 return fPropagator[idir](row, column);
00178 }
00179
00180
00181 void KalmanPlaneSR::SetPropagatorMatrixValue(Int_t idir, Int_t row,
00182 Int_t column, Double_t value)
00183 {
00184 fPropagator[idir](row,column) = value;
00185 return;
00186 }
00187
00188
00189 Double_t KalmanPlaneSR::GetPreCovarianceMatrixValue(Int_t idir, Int_t row,
00190 Int_t column) const
00191 {
00192 return fPreCovariance[idir](row, column);
00193 }
00194
00195
00196 void KalmanPlaneSR::SetPreCovarianceMatrixValue(Int_t idir, Int_t row,
00197 Int_t column, Double_t value)
00198 {
00199 fPreCovariance[idir](row,column) = value;
00200 return;
00201 }
00202
00203
00204 Double_t KalmanPlaneSR::GetFilCovarianceMatrixValue(Int_t idir, Int_t row,
00205 Int_t column) const
00206 {
00207 return fFilCovariance[idir](row, column);
00208 }
00209
00210
00211 void KalmanPlaneSR::SetFilCovarianceMatrix(Int_t idir, Double_t setMatrix[][5])
00212 {
00213
00214 for(Int_t i = 0; i<5; ++i){
00215 for(Int_t j = 0; j<5; ++j){
00216 fFilCovariance[idir](i,j) = setMatrix[i][j];
00217 }
00218 }
00219
00220 return;
00221 }
00222
00223
00224 void KalmanPlaneSR::SetFilCovarianceMatrixValue(Int_t idir, Int_t row,
00225 Int_t column, Double_t value)
00226 {
00227 fFilCovariance[idir](row,column) = value;
00228 return;
00229 }
00230
00231
00232 Double_t KalmanPlaneSR::GetNoiseMatrixValue(Int_t idir, Int_t row,
00233 Int_t column) const
00234 {
00235 return fNoise[idir](row, column);
00236 }
00237
00238
00239 void KalmanPlaneSR::SetNoiseMatrixValue(Int_t idir, Int_t row,
00240 Int_t column, Double_t value)
00241 {
00242 fNoise[idir](row,column) = value;
00243 return;
00244 }
00245
00246
00247 Double_t KalmanPlaneSR::GetPreChi2(Int_t idir) const
00248 {
00249 return fPreChi2[idir];
00250 }
00251
00252
00253 void KalmanPlaneSR::SetPreChi2(Double_t chi2, Int_t idir)
00254 {
00255 fPreChi2[idir] = chi2;
00256 return;
00257 }
00258
00259
00260 Double_t KalmanPlaneSR::GetFilChi2(Int_t idir) const
00261 {
00262 return fFilChi2[idir];
00263 }
00264
00265
00266 void KalmanPlaneSR::SetFilChi2(Double_t chi2, Int_t idir)
00267 {
00268 fFilChi2[idir] = chi2;
00269 return;
00270 }
00271
00272
00273 Int_t KalmanPlaneSR::GetZDir() const
00274 {
00275 return fZDir;
00276 }
00277
00278
00279 void KalmanPlaneSR::SetZDir(Int_t idir)
00280 {
00281 fZDir = idir;
00282 return;
00283 }
00284
00285
00286 Double_t KalmanPlaneSR::GetRange() const
00287 {
00288 return fRange;
00289 }
00290
00291
00292 void KalmanPlaneSR::SetRange(Double_t range)
00293 {
00294 fRange = range;
00295 return;
00296 }
00297
00298
00299 TrackClusterSR* KalmanPlaneSR::GetTrackCluster() const
00300 {
00301 return fTrackCluster;
00302 }
00303
00304
00305 void KalmanPlaneSR::SetTrackCluster(TrackClusterSR *cluster)
00306 {
00307 fTrackCluster = cluster;
00308 return;
00309 }
00310
00311
00312
00313 #define KP_TEST_ARRAY_EQUALITY(__rhs, __arr_member, __arr_len) \
00314 { \
00315 for(int __i=0; __i < __arr_len; __i++) { \
00316 if( !(this->__arr_member[__i] == __rhs->__arr_member[__i]) ) { \
00317 return false; \
00318 } \
00319 } \
00320 }
00321
00322 #define KP_TEST_EQUALITY(__rhs, __member) \
00323 { \
00324 if( !(this->__member == __rhs->__member) ) { \
00325 return false; \
00326 } \
00327 }
00328
00329
00330 Bool_t KalmanPlaneSR::IsEquivalent(const TObject *rkp) const
00331 {
00332 const KalmanPlaneSR *rhs = dynamic_cast<const KalmanPlaneSR*>(rkp);
00333
00334 KP_TEST_ARRAY_EQUALITY(rhs, fPreState, 2);
00335 KP_TEST_ARRAY_EQUALITY(rhs, fFilState, 2);
00336 KP_TEST_ARRAY_EQUALITY(rhs, fGain, 2);
00337 KP_TEST_ARRAY_EQUALITY(rhs, fPropagator, 2);
00338 KP_TEST_ARRAY_EQUALITY(rhs, fPreCovariance, 2);
00339 KP_TEST_ARRAY_EQUALITY(rhs, fFilCovariance, 2);
00340 KP_TEST_ARRAY_EQUALITY(rhs, fNoise, 2);
00341 KP_TEST_ARRAY_EQUALITY(rhs, fPreChi2, 2);
00342 KP_TEST_ARRAY_EQUALITY(rhs, fFilChi2, 2);
00343 KP_TEST_ARRAY_EQUALITY(rhs, fPredictPlane, 2);
00344 KP_TEST_EQUALITY( rhs, fZDir );
00345 KP_TEST_EQUALITY( rhs, fRange );
00346
00347 return true;
00348 }
00349
00350 XXXITRIMP(KalmanPlaneSR)
