00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017 #include <assert.h>
00018 #include "TrkFitter/TrkHelixFitter.h"
00019 #include "TrkBase/TrkSimpTraj.h"
00020 #include "TrkBase/TrkParams.h"
00021 #include "TrkBase/TrkHitOnTrk.h"
00022 #include "CLHEP/Matrix/Vector.h"
00023 #include "CLHEP/Geometry/Point3D.h"
00024 #include "CLHEP/Matrix/Matrix.h"
00025 #include "CLHEP/Matrix/SymMatrix.h"
00026 #include "TrkBase/TrkEnums.h"
00027 #include "TrkBase/TrkErrCode.h"
00028 #include "TrkBase/TrkHotList.h"
00029 #include <vector>
00030 using std::cout;
00031 using std::endl;
00032
00033 bool TrkHelixFitter::m_debug = false;
00034 double TrkHelixFitter::nSigmaCut[43] = {
00035 10.,5.,5.,10., 10.,5.,5.,10.,
00036 10.,5.,5.,5., 5.,5.,5.,5., 5.,5.,5.,10.,
00037 10.,5.,5.,5., 5.,5.,5.,5., 5.,5.,5.,5., 5.,5.,5.,10.,
00038 10.,5.,5.,5., 5.,5.,10.
00039 };
00040
00041 TrkHelixFitter::~TrkHelixFitter() {}
00042
00043
00044
00045 TrkHelixFitter::TrkHelixFitter(bool allowFlips, bool allowDrops):
00046
00047 TrkHitOnTrkUpdater()
00048 {
00049 _allowFlips = allowFlips;
00050 _allowDrops = allowDrops;
00051 _lastChisq = -1.;
00052 }
00053
00054
00055 TrkHelixFitter::TrkHelixFitter(const TrkHelixFitter& right):
00056
00057 TrkHitOnTrkUpdater()
00058 {
00059 _allowFlips = right._allowFlips;
00060 _allowDrops = right._allowDrops;
00061 _lastChisq = -1.;
00062 }
00063
00064
00065 TrkHelixFitter&
00066 TrkHelixFitter::operator=(const TrkHelixFitter& right)
00067
00068 {
00069 if (&right == this) return *this;
00070 _allowFlips = right._allowFlips;
00071 _allowDrops = right._allowDrops;
00072 _lastChisq = right._lastChisq;
00073
00074 return *this;
00075 }
00076
00077
00078 void
00079 TrkHelixFitter::setFittingPar(bool allowFlips, bool allowDrops) {
00080
00081 _allowFlips = allowFlips;
00082 _allowDrops = allowDrops;
00083 }
00084
00085
00086 TrkErrCode
00087 TrkHelixFitter::fit(TrkHotList& hitlist,
00088 TrkSimpTraj& theTraj) {
00089
00090
00091
00092
00093
00094
00095
00096
00097
00098
00099
00100
00101
00102
00103
00104
00105 bool permitFlips = _allowFlips;
00106 bool lPickHits = _allowDrops;
00107
00108
00109 int i;
00110 TrkErrCode status(TrkErrCode::succeed);
00111 int lPicked = 0;
00112
00113 register double chisqold;
00114 double chisqnew, chichange;
00115 double chitest = 0.01;
00116 int nZ = 0, nXY = 0;
00117 int nActive = 0;
00118
00119
00120
00121
00122
00123
00124
00125
00126
00127 setLastChisq(-1.);
00128
00129
00130
00131
00132
00133
00134
00135
00136
00137
00138
00139
00140
00141
00142
00143
00144
00145
00146
00147
00148
00149 int nhits = hitlist.nHit();
00150 std::vector<double> delChi(nhits,0);
00151 std::vector<std::vector<double> > deriv(nhits);
00152
00153 TrkParams ¶ms = *(theTraj.parameters());
00154
00155 int npar = theTraj.nPar();
00156
00157
00158
00159
00160 bool l3d = (npar > 3);
00161 const int minZ = l3d ? 2 : 0;
00162 const int minXY = npar - minZ;
00163 const int minAct = minZ + minXY;
00164
00165 HepSymMatrix vparam(npar,0);
00166 HepVector diffsum(npar);
00167 HepVector delpar(npar);
00168
00169 std::vector<std::vector<double> >::iterator ideriv = deriv.begin();
00170 std::vector<double>::iterator idelChi = delChi.begin();
00171 assert(((int)deriv.size()) ==(hitlist.end()-hitlist.begin()));
00172 for (TrkHotList::nc_hot_iterator ihit = hitlist.begin(); ihit != hitlist.end(); ++ihit,++ideriv,++idelChi) {
00173 ideriv->resize(npar);
00174 if (ihit->isActive()) {
00175 nActive++;
00176 if (ihit->whatView() == TrkEnums::xyView) nXY++;
00177 else if (ihit->whatView() == TrkEnums::zView) nZ++;
00178 else if (ihit->whatView() == TrkEnums::bothView) {
00179 nZ++;
00180 nXY++;
00181 }
00182 }
00183
00184
00185
00186
00187
00188 }
00189 if (nXY < minXY || nZ < minZ || nActive < minAct) {
00190 status.setFailure(11,"Not enough hits in TrkHelixFitter! ");
00191 return status;
00192 }
00193
00194
00195
00196
00197
00198
00199
00200
00201
00202
00203
00204
00205
00206
00207 HepVector derivs(npar);
00208
00209 TrkErrCode calcResult;
00210
00211 size_t itermax = 12;
00212 for (size_t iter = 1; iter <= itermax; iter++) {
00213 bool mustIterate(false);
00214 chisqold = 0.0;
00215 for (i = 0; i < npar; i++) diffsum[i] = 0.0;
00216 vparam *= 0.0;
00217
00218
00219 std::vector<std::vector<double> >::iterator ideriv = deriv.begin();
00220 std::vector<double>::iterator idelChi = delChi.begin();
00221 assert(((int)deriv.size())==(hitlist.end()-hitlist.begin()));
00222 for (TrkHotList::nc_hot_iterator ihit = hitlist.begin(); ihit != hitlist.end(); ++ihit,++ideriv,++idelChi) {
00223
00224
00225 calcResult = updateMeasurement(*ihit,0,!permitFlips);
00226 double deltaChiNew;
00227 if (calcResult.success()) {
00228 if (iter < 2) {
00229 calcResult = ihit->getFitStuff(derivs, deltaChiNew);
00230 for (i=0; i<npar; ++i) (*ideriv)[i] = derivs[i];
00231
00232
00233
00234
00235
00236
00237 } else {
00238 calcResult = ihit->getFitStuff(deltaChiNew);
00239 }
00240 }
00241 if (calcResult.failure()) {
00242 if(m_debug){
00243 cout<<"ErrMsg(warning) TrkHelixFitter:"
00244 << "unable to getFitStuff for hit " << *ihit << endl;
00245 }
00246 ihit->setUsability(false);
00247 continue;
00248 }
00249 mustIterate = (mustIterate || (calcResult.success() != 1));
00250 *idelChi = deltaChiNew;
00251 if(m_debug){
00252 cout << (ihit-hitlist.begin());
00253 ihit->print(std::cout);
00254 cout << " dChi " << *idelChi
00255 << " amb " << ihit->ambig()
00256 << " resid " << ihit->resid()
00257 << " rms " << ihit->hitRms()
00258 << " hitlen " << ihit->hitLen()
00259 << " fltlen " << ihit->fltLen() << endl;
00260 }
00261 if (ihit->isActive() == false) {
00262 if(m_debug) std::cout<<"SKIP not active hit"<< std::endl;
00263 continue;
00264 }
00265 chisqold += deltaChiNew * deltaChiNew;
00266
00267 for (i = 0; i < npar; ++i) {
00268 diffsum[i] += (*ideriv)[i] * deltaChiNew;
00269 for (int j = 0; j < i+1; ++j) {
00270 vparam.fast(i+1,j+1) += (*ideriv)[i] * (*ideriv)[j];
00271 }
00272 }
00273 }
00274
00275
00276
00277 int ierr;
00278 vparam.invert(ierr);
00279 if (ierr) {
00280 if(m_debug){
00281 cout<<"ErrMsg(warning) TrkHelixFitter:"
00282 << "Matrix inversion failed " << endl;
00283 }
00284 status.setFailure(12, "Matrix inversion failed in TrkHelixFitter");
00285
00286 }
00287 delpar = vparam * (-diffsum);
00288 if(m_debug){
00289 cout << " delpar = "<<delpar << endl;
00290 }
00291
00292
00293 if (fabs(delpar[1]) > 1.) {
00294 if(m_debug){
00295 cout<<"ErrMsg(warning) TrkHelixFitter:"
00296 << "Pathological fit " << endl;
00297 }
00298 status.setFailure(13, "Pathological fit in TrkHelixFitter.");
00299
00300 }
00301
00302 for (i = 0; i < npar; ++i) params.parameter()[i] += delpar[i];
00303 if(m_debug){
00304 cout << " params "<<params.parameter() << endl;
00305 }
00306
00307
00308
00309
00310
00311 chisqnew = 0.0;
00312 lPicked = 0;
00313 double bigDelChi = 0.0;
00314 TrkHotList::nc_hot_iterator bigHit = hitlist.end();
00315
00316 mustIterate = (mustIterate || (iter <= 2 && lPickHits));
00317 ideriv = deriv.begin();
00318 idelChi = delChi.begin();
00319 for (TrkHotList::nc_hot_iterator ihit = hitlist.begin(); ihit != hitlist.end(); ++ihit,++ideriv,++idelChi) {
00320 if(m_debug) {
00321 ihit->print(std::cout);
00322 }
00323 if(!ihit->isUsable()){
00324 if(m_debug) { std::cout<<"hit NOT usable "<< std::endl; }
00325 continue;
00326 }
00327
00328 for (i = 0; i < npar; i++) {
00329 *idelChi += (*ideriv)[i] * delpar[i];
00330 }
00331 if (ihit->isActive()) chisqnew += *idelChi * *idelChi;
00332
00333
00334 if (!mustIterate && lPickHits) {
00335 double abDelChi = fabs(*idelChi);
00336
00337 if (abDelChi <= nSigmaCut[ihit->layerNumber()] ) {
00338 if(m_debug){
00339 std::cout<< "abDelChi "<<abDelChi
00340 <<"<?"<<nSigmaCut[ihit->layerNumber()] << std::endl;
00341 }
00342 if (ihit->isActive() == 0) {
00343 ihit->setActivity(1);
00344 if(m_debug){ cout << "set ACTIVE, Added " << endl; }
00345 lPicked = 1;
00346 nActive++;
00347 if (ihit->whatView() == TrkEnums::xyView) nXY++;
00348 else if (ihit->whatView() == TrkEnums::zView) nZ++;
00349 else if (ihit->whatView() == TrkEnums::bothView) {
00350 nZ++;
00351 nXY++;
00352 }
00353 }
00354 } else {
00355 if (ihit->isActive()) {
00356 if (abDelChi > bigDelChi) {
00357 if(m_debug){
00358 std::cout<<"bigest set INACTIVE, abDelChi = "<<abDelChi
00359 <<">"<<nSigmaCut[ihit->layerNumber()] <<" bigDelChi=" <<bigDelChi<< std::endl; }
00360 bigDelChi = abDelChi;
00361 bigHit = ihit;
00362 }
00363 }
00364 }
00365 }
00366 }
00367
00368
00369 if (lPickHits) {
00370 int lDrop = 0;
00371 if (bigHit != hitlist.end() && (nActive > minAct)) {
00372 if ( bigHit->whatView() == TrkEnums::xyView && nXY > minXY) {
00373 nXY--;
00374 lDrop = 1;
00375 } else if ( bigHit->whatView() == TrkEnums::zView && nZ > minZ) {
00376 nZ--;
00377 lDrop = 1;
00378 } else if ( bigHit->whatView() == TrkEnums::bothView && nZ > minZ &&
00379 nXY > minXY) {
00380 nZ--;
00381 nXY--;
00382 lDrop = 1;
00383 }
00384 if (lDrop == 1) {
00385 lPicked = 1;
00386 nActive--;
00387 bigHit->setActivity(0);
00388 if(m_debug){
00389 std::cout<<"---deactivate hit!! delChi2="<<bigDelChi<< std::endl;
00390 std::cout<<"---";
00391 bigHit->print(std::cout);
00392 std::cout<<"--------------------!! "<< std::endl;
00393 }
00394 }
00395 }
00396 }
00397
00398
00399 chichange = chisqold - chisqnew;
00400 if(m_debug){
00401 cout << "chisq from "<<chisqold << " -> " << chisqnew << endl;
00402 }
00403 if (chichange < -0.5 && !mustIterate && lPicked == 0) {
00404 if(m_debug){
00405 cout<<"ErrMsg(warning)" << " blowing up: " << chichange << endl;
00406 }
00407
00408 setLastChisq(chisqnew);
00409 status.setFailure(1);
00410
00411 if(m_debug) std::cout<<"failure 1 "<< std::endl;
00412 break;
00413 } else if (chichange < chitest && !mustIterate && lPicked ==0){
00414
00415 status.setSuccess(1);
00416 setLastChisq(chisqnew);
00417 if(m_debug) std::cout<<"success 1 "<< std::endl;
00418 break;
00419 }
00420
00421 if (iter == itermax) {
00422 setLastChisq(chisqnew);
00423 status.setSuccess(2);
00424 if(m_debug) std::cout<<"success 2 "<< std::endl;
00425 }
00426 }
00427
00428
00429 params.covariance() = vparam;
00430
00431
00432
00433
00434
00435
00436
00437
00438
00439
00440
00441
00442
00443
00444
00445
00446
00447
00448
00449
00450
00451
00452
00453
00454
00455
00456
00457
00458
00459
00460
00461
00462
00463
00464
00465 return status;
00466 }