00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017 #include "TrkBase/TrkExchangePar.h"
00018 #include "TrkBase/TrkRecoTrk.h"
00019 #include "MdcTrkRecon/MdcSegGrouper.h"
00020 #include "MdcTrkRecon/mdcWrapAng.h"
00021 #include "MdcTrkRecon/MdcSegInfo.h"
00022 #include "MdcTrkRecon/MdcSeg.h"
00023 #include "MdcTrkRecon/mdcTwoVec.h"
00024 #include "MdcTrkRecon/mdcTwoInv.h"
00025 #include "MdcTrkRecon/MdcTrack.h"
00026 #include "MdcData/MdcHitUse.h"
00027 #include "MdcGeom/MdcDetector.h"
00028 #include "MdcData/MdcHit.h"
00029 #include "CLHEP/Alist/AList.h"
00030 #include "TrkBase/TrkRep.h"
00031 #include "MdcGeom/Constants.h"
00032 #include "MdcTrkRecon/MdcSegInfoSterO.h"
00033 #include "MdcTrkRecon/MdcLine.h"
00034 #include "BField/BField.h"
00035
00036
00037 #include "AIDA/IHistogram1D.h"
00038 extern AIDA::IHistogram1D* g_maxSegChisqAx;
00039 extern AIDA::IHistogram1D* g_maxSegChisqSt;
00040
00041 using std::cout;
00042 using std::endl;
00043
00044
00045 MdcSegGrouper::~MdcSegGrouper() {
00046
00047 delete [] segList;
00048 delete [] combList;
00049 delete [] currentSeg;
00050 delete [] leaveGap;
00051 delete [] gapCounter;
00052 delete [] firstGood;
00053 delete [] firstBad;
00054 if (isValid != 0) {
00055 for (int i = 0; i < nDeep; i++) {
00056 delete [] isValid[i];
00057 }
00058 delete [] isValid;
00059 }
00060
00061 }
00062
00063 MdcSegGrouper::MdcSegGrouper(const MdcDetector *gm, int nd, int debug) {
00064
00065 nDeep = nd;
00066 int nsuper = gm->nSuper();
00067 segList = new HepAList<MdcSeg>[nsuper];
00068 currentSeg = new int[nDeep];
00069 leaveGap = new bool[nDeep];
00070 gapCounter = new int[nDeep];
00071 combList = new HepAList<MdcSeg> * [nDeep];
00072 _gm = gm;
00073 firstGood = new int[nDeep];
00074 firstBad = new int[nDeep];
00075 lTestGroup = false;
00076 lTestSingle = false;
00077 _debug = debug;
00078 }
00079
00080
00081
00082
00083 int MdcSegGrouper::nextGroup( MdcSeg **segGroup, bool printit ) {
00084
00085
00086
00087
00088 int iply;
00089 for (iply = nPlyFilled; iply < nDeep; iply++) {
00090 segGroup[iply] = 0;
00091 }
00092
00093 restart:
00094 if (printit) cout <<endl<< "MdcSegGrouper::nextGroup starting group finder, nply = " << nPlyFilled << endl;
00095 int nFound = 0;
00096 bool incrementNext = true;
00097
00098 for (iply = 0; iply < nPlyFilled; iply++) {
00099 segGroup[iply] = 0;
00100 if (!incrementNext && currentSeg[iply] >= firstGood[iply]) {
00101 if(printit) std::cout<< " reach end of list, break." << iply<< std::endl;
00102 break;
00103 }
00104
00105 if (leaveGap[iply]) {
00106 if(printit) std::cout<< " leaveGap " <<std::endl;
00107
00108 if (iply == nPlyFilled - 1 && incrementNext) {
00109
00110 iply = -1;
00111 resetSegCounters();
00112 int lDone = updateGap();
00113 if (lDone) {
00114
00115 nNull++;
00116 if (nNull > maxNull) return 0;
00117 resetGap(nNull);
00118 updateGap();
00119 }
00120 }
00121 continue;
00122 }
00123 incrementNext = false;
00124
00125
00126 while (1) {
00127 currentSeg[iply]++;
00128 if (currentSeg[iply] == firstBad[iply]) {
00129 incrementNext = true;
00130 currentSeg[iply] = firstGood[iply];
00131 if(printit) { std::cout<< "reach end of segs "<<std::endl; }
00132 if (iply == nPlyFilled - 1) {
00133
00134 iply = -1;
00135 resetSegCounters();
00136 int lDone = updateGap();
00137 if (lDone) {
00138
00139 nNull++;
00140 if (nNull > maxNull) {
00141 if(printit) { std::cout<<endl<< " All done! "<<std::endl; }
00142 return 0;
00143 }
00144 resetGap(nNull);
00145 updateGap();
00146 }
00147 }
00148 break;
00149 }
00150
00151 if(printit){
00152 std::cout<< "ply " << iply<< " seg "<<currentSeg[iply]<<": ";
00153 (*combList[iply])[currentSeg[iply]]->plotSeg();
00154 if((*combList[iply])[currentSeg[iply]]->superlayer()->whichView()!= 0){
00155 MdcSegInfoSterO* info = (MdcSegInfoSterO *) (*combList[iply])[currentSeg[iply]]->info();
00156 std::cout<< " ct " << info->ct();
00157 }
00158 }
00159
00160
00161 if( (*combList[iply])[currentSeg[iply]]->segUsed()) {
00162 if(printit) {
00163 std::cout<< std::endl<<" Used??";
00164 (*combList[iply])[currentSeg[iply]]->plotSeg();
00165 }
00166 if(printit) { std::cout<< " segUsed! SKIP "<<std::endl; }
00167 continue;
00168
00169 }
00170
00171
00172 if (lTestSingle) {
00173 assert(isValid != 0);
00174 assert(isValid[iply] != 0);
00175 int invalid = (isValid[iply][currentSeg[iply]] == false);
00176 if(printit) {
00177 if(invalid){ std::cout<<" invalid "<< std::endl;
00178 }else{ std::cout<<" KEEP 1 "<< std::endl; }
00179 }
00180 if (invalid) continue;
00181 }else{
00182 if(printit) std::cout<<" KEEP 2 "<< std::endl;
00183 }
00184
00185
00186 break;
00187
00188 }
00189 }
00190
00191
00192 for (iply = 0; iply < nPlyFilled; iply++) {
00193 if (leaveGap[iply]) {
00194 segGroup[iply] = 0;
00195 } else {
00196 segGroup[iply] = (*combList[iply])[currentSeg[iply]];
00197 if (lTestGroup && nFound > 1) {
00198 int lBad = incompWithGroup(segGroup, segGroup[iply], iply);
00199 if(printit){
00200 if(lBad) std::cout<<" incompWithGroup Bad! restart" << std::endl;
00201
00202 }
00203 if (lBad) goto restart;
00204 }
00205 nFound++;
00206 }
00207 }
00208
00209 if (printit) std::cout<< "nextGoup() found " << nFound << " segment(s)"<<std::endl;
00210 return nFound;
00211 }
00212
00213
00214 void MdcSegGrouper::resetGap(int nGap) {
00215
00216
00217 for (int i = 0; i < nPlyFilled; i++) {
00218 gapCounter[i] = nGap - 1 - i;
00219 }
00220 gapCounter[0]--;
00221
00222 return;
00223 }
00224
00225
00226 int MdcSegGrouper::updateGap() {
00227
00228 if (nNull == 0) return 1;
00229
00230 for (int i = 0; i < nPlyFilled; i++) {
00231 leaveGap[i] = false;
00232 }
00233 for (int igap = 0; igap < nNull; igap++) {
00234 gapCounter[igap]++;
00235 if (gapCounter[igap] == nPlyFilled - igap) {
00236
00237
00238 int inext = igap + 1;
00239 while (1) {
00240 if (inext >= nNull) return 1;
00241 if (gapCounter[inext] + inext + 1 < nPlyFilled) {
00242
00243 gapCounter[igap] = gapCounter[inext] + inext + 1 - igap;
00244 break;
00245 }
00246 inext++;
00247 }
00248 }
00249 else {
00250
00251 break;
00252 }
00253 }
00254
00255 for (int j = 0; j < nNull; j++) {
00256 leaveGap[gapCounter[j]] = true;
00257 }
00258 return 0;
00259
00260 }
00261
00262 void MdcSegGrouper::resetSegCounters() {
00263
00264 for (int i = 0; i < nPlyFilled; i++) {
00265 currentSeg[i] = firstGood[i] - 1;
00266 }
00267 }
00268
00269
00270 int MdcSegGrouper::combineSegs(MdcTrack*& trk, MdcSeg *seed,
00271 TrkContext& context, double t0, double maxSegChisqO, int combineByFitHits) {
00272
00273
00274
00275 if (3 == _debug) std::cout<<std::endl<< "=====MdcSegGrouper::combineSegs=====" <<std::endl;
00276 bool lSeed = (seed != 0);
00277
00278 int success = 0;
00279 double qual;
00280 double qualBest = -1000.;
00281 int nSegFit = 0;
00282 int nSegBest = 0;
00283
00284 double param[2];
00285 double paramBest[2];
00286 double chiBest = 9999.;
00287 int nToUse = nPly();
00288 if (lSeed) nToUse++;
00289 MdcSeg **segGroup;
00290 MdcSeg **segGroupBest;
00291 segGroup = new MdcSeg * [nToUse];
00292 segGroupBest = new MdcSeg * [nToUse];
00293
00294
00295
00296
00297
00298
00299
00300 if ((3 == _debug)&&lSeed) {
00301 std::cout<<"seed segment: ";
00302 seed->plotSegAll();
00303 std::cout<< std::endl;
00304 }
00305 resetComb(seed);
00306
00307
00308
00309 double seedAngle[2] = {0.,0.};
00310 if (lSeed) {
00311 if (seed->info()->parIsAngle(0)) seedAngle[0] = seed->info()->par(0);
00312 if (seed->info()->parIsAngle(1)) seedAngle[1] = seed->info()->par(1);
00313 }
00314
00315 int iprint = ( 3 == _debug);
00316 int nInGroup = 0;
00317 while ( (nInGroup = nextGroup(segGroup, iprint)) != 0) {
00318
00319 if (lSeed) {
00320 segGroup[nToUse-1] = seed;
00321 nInGroup++;
00322 }
00323 if (nInGroup < 0) continue;
00324 if (nInGroup < 2) break;
00325 if (nInGroup < nSegBest) break;
00326
00327 if (3 == _debug || 11 == _debug) {
00328 cout << endl <<"-----found a segment group by nextGroup()----- nInGroup "<<nInGroup<<" nToUse "<<nToUse<<endl;
00329 for(int ii=0; ii<nToUse; ii++){ if(segGroup[ii]) {segGroup[ii]->plotSegAll(); cout<<endl;} }
00330
00331 }
00332
00333 double chisq =-999.;
00334 nSegFit=0;
00335
00336 if(lSeed){
00337
00338 chisq = calcParBySegs(segGroup, seedAngle, nToUse, qual, nSegFit, param);
00339 }else{
00340 if (combineByFitHits == 0){
00341 chisq = calcParBySegs(segGroup, seedAngle, nToUse, qual, nSegFit, param);
00342 }else{
00343
00344 const TrkFit* tkFit = trk->track().fitResult();
00345 double Bz = trk->track().bField().bFieldZ();
00346 TrkExchangePar par = tkFit->helix(0.0);
00347
00348 if (tkFit != 0) chisq = calcParByHits(segGroup, nToUse, par, qual,nSegFit, param, Bz);
00349
00350 if(chisq<=0){
00351
00352 chisq = calcParBySegs(segGroup, seedAngle, nToUse, qual, nSegFit, param);
00353 }
00354 }
00355 }
00356
00357
00358 if (chisq < 0.) continue;
00359 double chiDof = chisq/(2.*nSegFit - 2.);
00360
00361 if (g_maxSegChisqAx && lSeed ) { g_maxSegChisqAx->fill(chiDof); }
00362 if (g_maxSegChisqSt && !lSeed) { g_maxSegChisqSt->fill(chiDof); }
00363
00364
00365
00366 if (3 == _debug || 11 == _debug) {
00367 std::cout<< endl<<"chisq "<<chisq<<" nSegFit " << nSegFit<< " chiDof "<<chiDof<<std::endl;
00368 if(chiDof > maxSegChisqO) {
00369 cout << "***** DROP this group by chiDof "<<chiDof<<" > maxSegChisqO:"<<maxSegChisqO<<endl;
00370 }else{
00371 cout << "***** KEEP this group by chiDof "<<chiDof<<" <= maxSegChisqO:"<<maxSegChisqO<<endl;
00372 }
00373 }
00374
00375 if (chiDof > maxSegChisqO) continue;
00376
00377 success = 1;
00378 if (qual > qualBest) {
00379 qualBest = qual;
00380 nSegBest = nSegFit;
00381
00382 paramBest[0] = param[0];
00383 paramBest[1] = param[1];
00384 chiBest = chisq;
00385 for (int i = 0; i < nToUse; i++) {
00386 segGroupBest[i] = segGroup[i];
00387 }
00388 if (3 == _debug && 11 == _debug) std::cout<<"Keep as best group. param: phi0/z0 "
00389 <<paramBest[0]<< " cpa/ct "<<paramBest[1]<< std::endl;
00390 }
00391 }
00392
00393 if (success == 1) {
00394 if(3 == _debug || 11 == _debug) {
00395 std::cout<< endl<<"-----Parameter best group----- "<<std::endl;
00396 std::cout<< "paramBest "<<paramBest[0]<<" "<<paramBest[1]<< " chiBest " << chiBest<< std::endl;
00397 }
00398
00399 trk = storePar(trk, paramBest, chiBest, context, t0);
00400 transferHits(trk, nToUse, segGroupBest);
00401 }
00402 delete [] segGroupBest;
00403 delete [] segGroup;
00404 return success;
00405 }
00406
00407
00408 void MdcSegGrouper::transferHits(MdcTrack *trk, int nSegs, MdcSeg **segGroup) {
00409
00410
00411
00412
00413 double smallRad = 1000.;
00414 if (trk->firstLayer() != 0) smallRad = trk->firstLayer()->rMid();
00415 double bigRad = 0.;
00416 if (trk->lastLayer() != 0) bigRad = trk->lastLayer()->rMid();
00417
00418
00419
00420
00421
00422
00423 if(3 == _debug) std::cout<< endl<<"-----transferHits for this segGroup----- " <<std::endl;
00424 for (int i = 0; i < nSegs; i++) {
00425 if (segGroup[i] == 0) continue;
00426 if(3 == _debug) {
00427 cout << i << " "; segGroup[i]->plotSegAll(); cout<< endl;
00428 }
00429 segGroup[i]->setUsed();
00430 for (int ihit = 0; ihit < segGroup[i]->nHit(); ihit++) {
00431 MdcHitUse *aHit = segGroup[i]->hit(ihit);
00432 const MdcLayer *layer = aHit->mdcHit()->layer();
00433 double radius = layer->rMid();
00434 if (radius < smallRad) {
00435 smallRad = radius;
00436 trk->setFirstLayer(layer);
00437 }
00438
00439
00440 if (radius > bigRad && !trk->hasCurled()) {
00441 bigRad = radius;
00442 trk->setLastLayer(layer);
00443 }
00444
00445 double flt = radius;
00446 flt += 0.000001 * (aHit->mdcHit()->x() +aHit->mdcHit()->y());
00447
00448 aHit->setFltLen(flt);
00449
00450 TrkHitList* theHits = trk->track().hits();
00451
00452 if (theHits == 0) return;
00453
00454
00455 theHits->appendHit(*aHit);
00456
00457
00458
00459 }
00460 }
00461 }
00462
00463
00464 void MdcSegGrouper::dumpSegList(){
00465
00466 for(int islayer=0; islayer<11; islayer++){
00467 for(int i=0; i<segList[islayer].length(); i++){
00468 segList[islayer][i]->plotSegAll();
00469 std::cout<< std::endl;
00470 }
00471 }
00472 }
00473
00474
00475 double MdcSegGrouper::calcParBySegs(MdcSeg **segGroup, double seedAngle[2],
00476 int nToUse, double& qual, int& nSegFit, double param[2]) {
00477
00478 if (11 == _debug) std::cout<< "-----calculate group param by segment param-----"<<std::endl;
00479 double wgtmat[3], wgtinv[3];
00480 double wgtpar[2];
00481 double temvec[2], diff[2];
00482
00483 int nhit = 0;
00484 wgtmat[0] = wgtmat[1] = wgtmat[2] = wgtpar[0] = wgtpar[1] = 0.0;
00485
00486 int iPly;
00487 for (iPly = 0; iPly < nToUse; iPly++) {
00488 if (11 == _debug) {
00489
00490 }
00491 if (segGroup[iPly] == 0) continue;
00492 nSegFit++;
00493 MdcSegInfo *segInfo = segGroup[iPly]->info();
00494
00495 for (int i = 0; i < 3; i++) wgtmat[i] += (segInfo->inverr())[i];
00496 for (int k = 0; k < 2; k++) {
00497 param[k] = segInfo->par(k);
00498
00499 if (segInfo->parIsAngle(k)) {
00500 param[k] = mdcWrapAng(seedAngle[k], param[k]);
00501 }
00502 }
00503
00504 mdcTwoVec( segInfo->inverr(), param, temvec );
00505 wgtpar[0] += temvec[0];
00506 wgtpar[1] += temvec[1];
00507 if(11 == _debug) {
00508 std::cout<<0<<": param "<<param[0]<<" inverr "<< segInfo->inverr()[0]<<" par*W "<<temvec[0]<<std::endl;
00509 std::cout<<1<<": param "<<param[1]<<" "<<1/param[1]<<" inverr "<< segInfo->inverr()[1]<<" par*W "<<temvec[1]<<std::endl;
00510 std::cout<< " " <<std::endl;
00511 }
00512 nhit += segGroup[iPly]->nHit();
00513 }
00514
00515
00516 int error = mdcTwoInv(wgtmat,wgtinv);
00517 if (error && (11 == _debug)) {
00518 cout << "ErrMsg(warning) "
00519 << "failed matrix inversion in MdcTrackList::combineSegs" << endl;
00520
00521 return -999.;
00522 }
00523 mdcTwoVec( wgtinv, wgtpar, param );
00524 if(11 == _debug) {
00525 std::cout<< " param of wgtinv * wgtpar" << std::endl;
00526 std::cout<<0<<": param "<<param[0]<< std::endl;
00527 std::cout<<1<<": param "<<param[1]<<" "<<1/param[1]<< std::endl;
00528 std::cout<< " " <<std::endl;
00529 }
00530
00531
00532
00533
00534 if(11 == _debug)cout<<endl<<"-- Calculate track & chisq for this group "<<endl;
00535
00536
00537
00538 double chisq = 0.0;
00539 for (iPly = 0; iPly < nToUse; iPly++) {
00540 if (segGroup[iPly] == 0) continue;
00541 MdcSegInfo *segInfo = segGroup[iPly]->info();
00542 for (int j = 0; j < 2; j++) {
00543 double temPar;
00544 if (segInfo->parIsAngle(j)) {
00545 temPar = mdcWrapAng(seedAngle[j], segInfo->par(j));
00546 } else {
00547 temPar = segInfo->par(j);
00548 }
00549 if(11 == _debug) {
00550 std::cout<<" segPar"<<j<<" "<<temPar<< std::endl;
00551 }
00552 diff[j] = temPar - param[j];
00553 }
00554
00555 if(11 == _debug) {
00556 std::cout<<"inverr " <<segInfo->inverr()[0]<<" "
00557 <<segInfo->inverr()[1] <<" "<<segInfo->inverr()[2] << std::endl;
00558 std::cout<<"errmat " <<segInfo->errmat()[0]<< " "
00559 <<segInfo->errmat()[1] << " "<<segInfo->errmat()[2] << std::endl;
00560 std::cout<<"diff " <<diff[0]<<" "<<diff[1]<<" temvec "<<temvec[0]<<" "<<temvec[1]<< std::endl;
00561 std::cout<< std::endl;
00562 }
00563 mdcTwoVec( segInfo->inverr(), diff, temvec);
00564
00565 chisq += diff[0] * temvec[0] + diff[1] * temvec[1];
00566
00567 if(11 == _debug){
00568 std::cout<<iPly<<" chi2Add:"<<diff[0] * temvec[0] + diff[1] * temvec[1]<<" diff0 "<< diff[0]<< " vec0 "<<temvec[0]<<" diff1 "<< diff[1]<< " vec1 "<<temvec[1] << std::endl;
00569 }
00570 }
00571
00572 double chiDof = chisq/(2.*nSegFit - 2.);
00573 if (11 == _debug) {
00574 cout << "segment:"<<endl<<" chiDof "<<chiDof<<" chisq "<< chisq << " nhit " << nhit << " phi0/z0 " <<
00575 param[0] << " cpa/cot " << param[1] << " qual "<<(2. * nhit - chiDof) <<endl;
00576 }
00577
00578 qual = 2. * nhit - chiDof;
00579
00580
00581 return chisq;
00582 }
00583
00584
00585 double MdcSegGrouper::calcParByHits(MdcSeg **segGroup, int nToUse, const TrkExchangePar &par, double& qual, int& nSegFit, double param[2], double Bz) {
00586
00587
00588 int debug = false;
00589 if (11 == _debug ) debug = true;
00590 if (debug) std::cout<< "-----calculate group param by hit-----"<<std::endl;
00591 MdcLine span(50);
00592 MdcLine spanFit(50);
00593
00594 int nHit = 0;
00595 if (debug) std::cout<< "nToUse="<<nToUse<<std::endl;
00596 for (int iPly = 0; iPly < nToUse; iPly++) {
00597 if (segGroup[iPly] == 0) continue;
00598 nSegFit++;
00599 MdcSegInfoSterO *segInfo = dynamic_cast<MdcSegInfoSterO*> (segGroup[iPly]->info());
00600
00601 if(debug) std::cout<< "nHit in segment="<<segGroup[iPly]->nHit()<<std::endl;
00602 for (int ii=0,iHit=0; ii<segGroup[iPly]->nHit(); ii++){
00603
00604 if(debug)std::cout<< " calcParByHits ("<< segGroup[iPly]->hit(iHit)->mdcHit()->layernumber()<<","<<segGroup[iPly]->hit(iHit)->mdcHit()->wirenumber()<<")";
00605
00606
00607 int szCode = segInfo->zPosition(*(segGroup[iPly]->hit(iHit)),par,&span,iHit,segGroup[iPly]->bunchTime(),Bz);
00608 if(debug)std::cout<< " szCode "<<szCode;
00609 if(szCode>0&&debug) std::cout<< iHit<<" s "<< span.x[iHit]<< " z "<<span.y[iHit] <<" sigma "<<span.sigma[iHit];
00610 if(debug)std::cout<<std::endl;
00611
00612 spanFit.x[nHit]=span.x[iHit];
00613 spanFit.y[nHit]=span.y[iHit];
00614
00615 spanFit.sigma[nHit]=1.;
00616 if(debug)std::cout<< std::endl;
00617 iHit++;
00618 if(szCode>0) nHit++;
00619 }
00620 }
00621
00622 if(debug)std::cout<< __FILE__ << " " << __LINE__ << " nHit "<< nHit<<std::endl;
00623 if (nHit>0) spanFit.fit(nHit);
00624 else return -999;
00625
00626 param[0] = spanFit.intercept;
00627 param[1] = spanFit.slope;
00628 if(debug)std::cout<< "nHit "<<nHit<<" intercept(z0) "<<param[0]<< " slope(ct) " << param[1] <<std::endl;
00629
00630 qual = 2.*nHit - spanFit.chisq/(spanFit.nPoint - 2);
00631 if(debug)std::cout<< "chisq "<<spanFit.chisq<<" qual "<<qual<<std::endl;
00632
00633 return spanFit.chisq;
00634 }