#include <TrkHelixFitter.h>
Inheritance diagram for TrkHelixFitter:
|
||||||||||||
|
00045 : 00046 //------------------------------------------------------------------------ 00047 TrkHitOnTrkUpdater() 00048 { 00049 _allowFlips = allowFlips; 00050 _allowDrops = allowDrops; 00051 _lastChisq = -1.; 00052 }
|
|
|
00041 {}
|
|
|
00055 : 00056 //------------------------------------------------------------------------ 00057 TrkHitOnTrkUpdater() 00058 { 00059 _allowFlips = right._allowFlips; 00060 _allowDrops = right._allowDrops; 00061 _lastChisq = -1.; 00062 }
|
|
||||||||||||
|
|
|
|
|
|
|
|
|
||||||||||||
|
|
|
||||||||||||
|
00088 {
00089 //------------------------------------------------------------------------
00090 // Assumes that weight matrix is diagonal. */
00091 /* Least-squares fit; the measured
00092 quantity is the residual. The fit is accomplished by linearizing
00093 the equation, using the derivatives of the residual w/r/t the
00094 track parameters; because of this approximation, the fit may be iterated.
00095 The fitted parameters are given by:
00096 delta-param() = Vparam * Atran * Vyinv * delChi
00097 where Vyinv = covariance matrix for the measurements
00098 Atran = transpose of A
00099 A = matrix of derivatives of delChi wrt trk params
00100 (size = no. of params x no. of hits)
00101 Vparam = covariance (error)" matrix of the fitted parameters
00102 = (Atran * Vyinv * A)**-1
00103 */
00104
00105 bool permitFlips = _allowFlips;
00106 bool lPickHits = _allowDrops;
00107 // permitFlips = 1 => permit state changes like ambiguity flips
00108 // lPickHits = 1 => choose the best set of active hits on each iteration
00109 int i;
00110 TrkErrCode status(TrkErrCode::succeed);
00111 int lPicked = 0; // = 1 => have either picked up or dropped an active hit
00112 // on this iteration
00113 register double chisqold;
00114 double chisqnew, chichange;
00115 double chitest = 0.01; //delta(chi2) < chitest => fit has converged
00116 int nZ = 0, nXY = 0; // # active hits in each view
00117 int nActive = 0;
00118
00119 // vparam = Vparam defined above ( = symmetric matrix)
00120 // diffsum = Atran * Vyinv * delChi defined above (column vector)
00121 // iter = iteration loop index
00122 // itermax = max number of iterations
00123 // delpar = change in parameters during this iteration
00124 // chisqold, chisqnew = chisq before and after latest iteration
00125
00126 /***************************************************************************/
00127 setLastChisq(-1.);
00128 //bool shiftRef = false;//yzhang FIXME
00129 // HepPoint3D storePoint;
00130
00131 // Change reference point of trajectory to be at first hit -- reduces
00132 // numerical problems
00133 // double oldT0 = hitlist[0]->parentTrack()->trackT0();
00134 //if (shiftRef) {
00135 // double firstFlight = hitlist[0]->fltLen();
00136 // double newTime = hitlist[0]->parentTrack()->fitResult()->arrivalTime(firstFlight);
00137 // hitlist[0]->parentTrack()->resetT0(newTime);
00138 // Point3D.here = theTraj.position(firstFlight);
00139 //
00140 // storePoint = here;
00141 // DifPoint dfPos;
00142 // DifVector dfDir, dfDelDir;
00143 // theTraj.getDFInfo(firstFlight, dfPos, dfDir, dfDelDir);
00144 //
00145 // theTraj.changePoint(here, fltOffset);
00146 //}
00147
00148 //*** Things that don't change with each iteration
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 // int npar = params.nPar();//yzhang temp
00155 int npar = theTraj.nPar();//yzhang temp
00156
00157 // Decide minimum numbers of hits required. This could turn out to be wrong
00158 // someday.
00159
00160 bool l3d = (npar > 3); // I hope always true
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 // Update the Hots to reflect new reference point
00185 //if (shiftRef) {
00186 // ihit->setFltLen( ihit->fltLen() - fltOffset );
00187 //}
00188 } //end loop over hits
00189 if (nXY < minXY || nZ < minZ || nActive < minAct) {
00190 status.setFailure(11,"Not enough hits in TrkHelixFitter! ");
00191 return status;
00192 }
00193
00194
00195 //if (shiftRef) {
00196 // double firstFlight = hitlist[0]->fltLen();
00197 // Point3D.here = theTraj.position(firstFlight);
00198 // DifPoint dfPos;
00199 // DifVector dfDir, dfDelDir;
00200 // theTraj.getDFInfo(firstFlight, dfPos, dfDir, dfDelDir);
00201 // double dummy = 0.;
00202 // hitlist[0]->updateFitStuff(dummy, 0, !permitFlips);
00203 // hitlist[0]->updateMeasurement();
00204 // hitlist[0]->updateFitStuff(dummy, 0, !permitFlips);
00205 //}
00206 // static HepVector derivs(npar);//yzhang temp
00207 HepVector derivs(npar);//zhang change
00208 // HepVector derivs(npar);//yzhang temp
00209 TrkErrCode calcResult;
00210 //**** Iterate fit.
00211 size_t itermax = 12;
00212 for (size_t iter = 1; iter <= itermax; iter++) {
00213 bool mustIterate(false); // flag to force another iteration
00214 chisqold = 0.0;
00215 for (i = 0; i < npar; i++) diffsum[i] = 0.0;
00216 vparam *= 0.0; // dumb way of clearing matrix
00217
00218 /* Loop over hits, accumulate sums, calculate chisq for current params. */
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 // Ask the hit to do the calculations
00225 calcResult = updateMeasurement(*ihit,0,!permitFlips);
00226 double deltaChiNew;
00227 if (calcResult.success()) {
00228 if (iter < 2) { // FIXME? only update derivatives at first iteration...
00229 calcResult = ihit->getFitStuff(derivs, deltaChiNew);
00230 for (i=0; i<npar; ++i) (*ideriv)[i] = derivs[i];
00231 //if(m_debug){
00232 // std::cout<<"in TrkHelixFitter " <<std::endl;//yzhang deubg
00233 // cout << "deriv: ";
00234 // for (i=0;i<npar;++i) cout << (*ideriv)[i] << " " ;
00235 // cout << endl;
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); // something bombed
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 } // end loop over hits
00274
00275
00276 //**** Calculate new paramters
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 //break;
00286 }
00287 delpar = vparam * (-diffsum);
00288 if(m_debug){
00289 cout << " delpar = "<<delpar << endl;
00290 }
00291 // The following test relies on having a fixed location for phi0 in
00292 // all simple params; it should be made robust somehow!!!
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 //break;
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 //***** Loop through the hits again, calculating the approx change
00308 // in residuals and chisq., and picking which hits should be active
00309 // for next iteration.
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)); // iterate until hit-dropping allowed
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 //double weight = ihit->weight(); // FIXME: why isn't weight used???
00328 for (i = 0; i < npar; i++) {
00329 *idelChi += (*ideriv)[i] * delpar[i];
00330 }
00331 if (ihit->isActive()) chisqnew += *idelChi * *idelChi;
00332
00333 // Hit-picking
00334 if (!mustIterate && lPickHits) {
00335 double abDelChi = fabs(*idelChi);
00336 //yzhang fix nSigmaCut for each layers 2010-04-13
00337 if (abDelChi <= nSigmaCut[ihit->layerNumber()] ) {
00338 if(m_debug){
00339 std::cout<< "abDelChi "<<abDelChi
00340 <<"<?"<<nSigmaCut[ihit->layerNumber()] << std::endl;//yzhang debug
00341 }
00342 if (ihit->isActive() == 0) {
00343 ihit->setActivity(1); // reactivate hit
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(m_debug){ std::cout<<"set INACTIVE, delChi2= "<<abDelChi<< std::endl; }
00356 if (ihit->isActive()) {
00357 if (abDelChi > bigDelChi) {
00358 bigDelChi = abDelChi;
00359 bigHit = ihit;
00360 }
00361 }
00362 }
00363 } // end if iter > 2 (hit-picking)
00364 } //end loop over hits
00365
00366 // Drop hit with worst residual
00367 if (lPickHits) {
00368 int lDrop = 0;
00369 if (bigHit != hitlist.end() && (nActive > minAct)) {
00370 if ( bigHit->whatView() == TrkEnums::xyView && nXY > minXY) {
00371 nXY--;
00372 lDrop = 1;
00373 } else if ( bigHit->whatView() == TrkEnums::zView && nZ > minZ) {
00374 nZ--;
00375 lDrop = 1;
00376 } else if ( bigHit->whatView() == TrkEnums::bothView && nZ > minZ &&
00377 nXY > minXY) {
00378 nZ--;
00379 nXY--;
00380 lDrop = 1;
00381 }
00382 if (lDrop == 1) {
00383 lPicked = 1;
00384 nActive--;
00385 bigHit->setActivity(0); // deactivate hit
00386 if(m_debug){
00387 std::cout<<"---deactivate hit!! delChi2="<<bigDelChi<< std::endl;
00388 std::cout<<"---";
00389 bigHit->print(std::cout);
00390 std::cout<<"--------------------!! "<< std::endl;
00391 }
00392 }
00393 }
00394 }// end if lPickHits
00395
00396 /* Test for convergence. */
00397 chichange = chisqold - chisqnew;
00398 if(m_debug){
00399 cout << "chisq from "<<chisqold << " -> " << chisqnew << endl;
00400 }
00401 if (chichange < -0.5 && !mustIterate && lPicked == 0) {
00402 if(m_debug){
00403 cout<<"ErrMsg(warning)" << " blowing up: " << chichange << endl;
00404 }
00405 /* It's blowing up. */
00406 setLastChisq(chisqnew);
00407 status.setFailure(1);
00408
00409 if(m_debug) std::cout<<"failure 1 "<< std::endl;
00410 break;
00411 } else if (chichange < chitest && !mustIterate && lPicked ==0){
00412 // We converged.
00413 status.setSuccess(1);
00414 setLastChisq(chisqnew);
00415 if(m_debug) std::cout<<"success 1 "<< std::endl;
00416 break;
00417 }
00418
00419 if (iter == itermax) {
00420 setLastChisq(chisqnew);
00421 status.setSuccess(2);
00422 if(m_debug) std::cout<<"success 2 "<< std::endl;
00423 }
00424 } /* end iteration loop */
00425
00426 // store the error matrix
00427 params.covariance() = vparam;
00428
00429 // Attempt to calculate deltaChisq for this hit (compared to leaving hit
00430 // out of the fit).
00431 /* chisqnew = 0;
00432 if (status.success()) {
00433 HepVector deltaAlpha(npar);
00434 for (ihit = 0; ihit < nhits; ihit++) {
00435 thisHot = hitlist(ihit);
00436 if (!thisHot->isActive()) continue;
00437 HepVector derivs(npar, 0);
00438 for (i = 0; i < npar; i++) {
00439 derivs[i] = deriv[ihit][i];
00440 }
00441 double weight = thisHot->weight();
00442 double resid = thisHot->resid();
00443 deltaAlpha = vparam * derivs;
00444 deltaAlpha *= (resid * weight);
00445 // cout << resid * resid * weight
00446 // << " " << resid * weight * dot(derivs, deltaAlpha) << endl;
00447 // thisHot->chi2Contrib = -dot(deltaAlpha, temp) + resid * resid * weight
00448 // + 2. * resid * weight * dot(derivs, deltaAlpha);
00449 }
00450 }
00451 */
00452
00453 // Change reference point back to origin
00454 //if (shiftRef) {
00455 // Point3D.home(0.,0.,0.);
00456 // theTraj.changePoint(home, fltOffset);
00457 // hitlist[0]->parentTrack()->resetT0(oldT0);
00458 //for (ihit = 0; ihit < nhits; ihit++) {
00459 // thisHot = hitlist[ihit];
00460 // thisHot->setFltLen( thisHot->fltLen() - fltOffset );
00461 // }
00462 //}
00463 return status;
00464 }
|
|
|
00038 {return _lastChisq;}
|
|
|
00038 {return _lastChisq;}
|
|
|
|
|
|
00068 {
00069 if (&right == this) return *this;
00070 _allowFlips = right._allowFlips;
00071 _allowDrops = right._allowDrops;
00072 _lastChisq = right._lastChisq;
00073
00074 return *this;
00075 }
|
|
|
00056 { return TrkBase::Functors::setActive(active); }
|
|
|
00056 { return TrkBase::Functors::setActive(active); }
|
|
||||||||||||
|
00044 {
00045 hot.setActive(active); }
|
|
||||||||||||
|
00044 {
00045 hot.setActive(active); }
|
|
||||||||||||
|
|
|
||||||||||||
|
00079 {
00080 //------------------------------------------------------------------------
00081 _allowFlips = allowFlips;
00082 _allowDrops = allowDrops;
00083 }
|
|
|
00047 {_lastChisq = l;}
|
|
|
00047 {_lastChisq = l;}
|
|
|
00054 { return TrkBase::Functors::setParent(parent); }
|
|
||||||||||||
|
00047 {
00048 hot._parentRep = parent;
00049 }
|
|
|
00054 { return TrkBase::Functors::setParent(parent); }
|
|
||||||||||||
|
00047 {
00048 hot._parentRep = parent;
00049 }
|
|
||||||||||||
|
00052 { return TrkBase::Functors::updateMeasurement(traj,maintainAmbiguity); }
|
|
||||||||||||||||
|
00042 { return hot.updateMeasurement(traj,maintainAmbiguity);}
|
|
||||||||||||
|
00052 { return TrkBase::Functors::updateMeasurement(traj,maintainAmbiguity); }
|
|
||||||||||||||||
|
00042 { return hot.updateMeasurement(traj,maintainAmbiguity);}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
Initial value: {
10.,5.,5.,10., 10.,5.,5.,10.,
10.,5.,5.,5., 5.,5.,5.,5., 5.,5.,5.,10.,
10.,5.,5.,5., 5.,5.,5.,5., 5.,5.,5.,5., 5.,5.,5.,10.,
10.,5.,5.,5., 5.,5.,10.
}
|
1.3.9.1