15 #include "../include/VertexFitterKalman.h"
16 #include "../include/interactionpoint.h"
17 #include "../include/vertexfitterlsm.h"
19 namespace vertex_lcfi {
namespace ZVTOP {
24 bool pDecreasing(
const TState& lhs,
const TState& rhs) {
25 return( fabs(lhs.track()->helixRep().invR())
26 < fabs(rhs.track()->helixRep().invR()) );
32 VertexFitterKalman::VertexFitterKalman() : m_useManualSeed(false), fNDF(-3), fChi2(0) {}
34 void VertexFitterKalman::fitVertex(
const std::vector<TrackState*> & Tracks,
38 double & ChiSquaredOfFit) {
42 for(
int i=0; i<6; ++i) fP[i] = 0.;
43 for(
int i=0; i<21; ++i) fC[i] = 0.;
44 fC[0] = fC[2] = fC[5] = 10000.;
53 std::vector<TrackState*>::const_iterator its;
54 for( its = Tracks.begin(); Tracks.end() != its; its++ )
56 TrackState* state = (*its);
57 TState myState(state);
58 fStates.push_back(myState);
65 if( Tracks.size() < 2 )
67 VertexFitterLSM fitterLSM;
68 if( m_useManualSeed ) fitterLSM.setSeed(m_manualSeed);
69 std::map<TrackState*,double> ChiSquaredOfTracks;
70 double ChiSquaredOfIP;
71 fitterLSM.fitVertex(Tracks, IP, Result, ResultError,
72 ChiSquaredOfFit, ChiSquaredOfTracks, ChiSquaredOfIP);
76 fC[0] = ResultError(0,0);
77 fC[1] = ResultError(0,1);
78 fC[2] = ResultError(1,1);
79 fC[3] = ResultError(0,2);
80 fC[4] = ResultError(1,2);
81 fC[5] = ResultError(2,2);
88 std::sort( fStates.begin(), fStates.end(), pDecreasing );
93 fVtxGuess[0] = 0.; fVtxGuess[1] = 0.; fVtxGuess[2] = 0.;
96 if (m_useManualSeed) {
97 fVtxGuess[0] = m_manualSeed(0);
98 fVtxGuess[1] = m_manualSeed(1);
99 fVtxGuess[2] = m_manualSeed(2);
103 if( fStates.size()>1 )
105 bool flag = estimateVertex(fVtxGuess);
108 fVtxGuess[0] = 0.; fVtxGuess[1] = 0.; fVtxGuess[2] = 0.;
111 if( fStates.size()==1 || fStates.size() == 0 )
114 fVtxGuess[0] = IP->position().x();
115 fVtxGuess[1] = IP->position().y();
116 fVtxGuess[2] = IP->position().z();
118 fC[0] = IP->errorMatrix()(0,0);
119 fC[1] = IP->errorMatrix()(0,1);
120 fC[2] = IP->errorMatrix()(1,1);
121 fC[3] = IP->errorMatrix()(0,2);
122 fC[4] = IP->errorMatrix()(1,2);
123 fC[5] = IP->errorMatrix()(2,2);
128 if( fStates.size() == 0 || (fStates.size() == 1 && !IP) ) {
129 Result(0) = fVtxGuess[0];
130 Result(1) = fVtxGuess[1];
131 Result(2) = fVtxGuess[2];
132 ResultError(0,0) = fC[0];
133 ResultError(0,1) = ResultError(1,0) = fC[1];
134 ResultError(1,1) = fC[2];
135 ResultError(0,2) = ResultError(2,0) = fC[3];
136 ResultError(1,2) = ResultError(2,1) = fC[4];
137 ResultError(2,2) = fC[5];
138 ChiSquaredOfFit = fChi2;
144 fP[0] = fVtxGuess[0]; fP[1] = fVtxGuess[1]; fP[2] = fVtxGuess[2];
147 fP[0] = IP->position().x();
148 fP[1] = IP->position().y();
149 fP[2] = IP->position().z();
150 fC[0] = IP->errorMatrix()(0,0);
151 fC[1] = IP->errorMatrix()(0,1);
152 fC[2] = IP->errorMatrix()(1,1);
153 fC[3] = IP->errorMatrix()(0,2);
154 fC[4] = IP->errorMatrix()(1,2);
155 fC[5] = IP->errorMatrix()(2,2);
160 for( std::vector<TState>::iterator it = fStates.begin(); fStates.end() != it; it++ )
163 TState* state = &(*it);
165 for(
int iter=0; iter<maxIter; iter++ )
168 double *ffP = fP, *ffC = fC;
171 state->GetMeasurement( fVtxGuess, m, mV );
177 double mSi[6] = { ffC[0]+mV[0],
178 ffC[1]+mV[1], ffC[2]+mV[2],
179 ffC[3]+mV[3], ffC[4]+mV[4], ffC[5]+mV[5] };
181 mS[0] = mSi[2]*mSi[5] - mSi[4]*mSi[4];
182 mS[1] = mSi[3]*mSi[4] - mSi[1]*mSi[5];
183 mS[2] = mSi[0]*mSi[5] - mSi[3]*mSi[3];
184 mS[3] = mSi[1]*mSi[4] - mSi[2]*mSi[3];
185 mS[4] = mSi[1]*mSi[3] - mSi[0]*mSi[4];
186 mS[5] = mSi[0]*mSi[2] - mSi[1]*mSi[1];
188 double s = ( mSi[0]*mS[0] + mSi[1]*mS[1] + mSi[3]*mS[3] );
189 s = ( s > 1.E-20 ) ? 1./s : 0 ;
191 mS[0]*=s; mS[1]*=s; mS[2]*=s;
192 mS[3]*=s; mS[4]*=s; mS[5]*=s;
197 double zeta[3] = { m[0]-ffP[0], m[1]-ffP[1], m[2]-ffP[2] };
201 double mCHt0[6], mCHt1[6], mCHt2[6];
203 mCHt0[0]=ffC[ 0] ; mCHt1[0]=ffC[ 1] ; mCHt2[0]=ffC[ 3] ;
204 mCHt0[1]=ffC[ 1] ; mCHt1[1]=ffC[ 2] ; mCHt2[1]=ffC[ 4] ;
205 mCHt0[2]=ffC[ 3] ; mCHt1[2]=ffC[ 4] ; mCHt2[2]=ffC[ 5] ;
206 mCHt0[3]=ffC[ 6]-mV[ 6]; mCHt1[3]=ffC[ 7]-mV[ 7]; mCHt2[3]=ffC[ 8]-mV[ 8];
207 mCHt0[4]=ffC[10]-mV[10]; mCHt1[4]=ffC[11]-mV[11]; mCHt2[4]=ffC[12]-mV[12];
208 mCHt0[5]=ffC[15]-mV[15]; mCHt1[5]=ffC[16]-mV[16]; mCHt2[5]=ffC[17]-mV[17];
212 double k0[6], k1[6], k2[6];
214 for(
int i=0;i<6;++i){
215 k0[i] = mCHt0[i]*mS[0] + mCHt1[i]*mS[1] + mCHt2[i]*mS[3];
216 k1[i] = mCHt0[i]*mS[1] + mCHt1[i]*mS[2] + mCHt2[i]*mS[4];
217 k2[i] = mCHt0[i]*mS[3] + mCHt1[i]*mS[4] + mCHt2[i]*mS[5];
222 if( iter<maxIter-1 ){
223 for(
int i=0; i<3; ++i)
224 fVtxGuess[i]= ffP[i] + k0[i]*zeta[0]+k1[i]*zeta[1]+k2[i]*zeta[2];
246 for(
int i=0; i<6; ++i )
247 fP[i] = ffP[i] + k0[i]*zeta[0] + k1[i]*zeta[1] + k2[i]*zeta[2];
252 for(
int i=0,k=0; i<6; ++i) {
253 for(
int j=0; j<=i; ++j,++k)
254 fC[k] = ffC[k] - (k0[i]*mCHt0[j] + k1[i]*mCHt1[j] + k2[i]*mCHt2[j] );
260 fChi2 += (mS[0]*zeta[0] + mS[1]*zeta[1] + mS[3]*zeta[2])*zeta[0]
261 + (mS[1]*zeta[0] + mS[2]*zeta[1] + mS[4]*zeta[2])*zeta[1]
262 + (mS[3]*zeta[0] + mS[4]*zeta[1] + mS[5]*zeta[2])*zeta[2];
274 for( std::vector<TState>::iterator it = fStates.begin();
275 fStates.end() != it; it++ )
277 TState* state = &(*it);
278 double chi2 = getDeviationFromVertex( state, fP, fC );
279 fChi2chain.push_back(chi2);
283 Result(0) = fP[0]; Result(1) = fP[1]; Result(2) = fP[2];
285 ResultError(0,0) = fC[0];
286 ResultError(0,1) = ResultError(1,0) = fC[1];
287 ResultError(1,1) = fC[2];
288 ResultError(0,2) = ResultError(2,0) = fC[3];
289 ResultError(1,2) = ResultError(2,1) = fC[4];
290 ResultError(2,2) = fC[5];
295 if( IP ) chi2sum += IP->chi2(Result);
298 ChiSquaredOfFit = fChi2;
303 void VertexFitterKalman::fitVertex(
const std::vector<TrackState*> & Tracks,
304 InteractionPoint* IP,
Vector3 & Result)
306 Matrix3x3 ResultError;
double ChiSquaredOfFit;
307 this->fitVertex(Tracks, IP, Result, ResultError, ChiSquaredOfFit);
311 void VertexFitterKalman::fitVertex(
const std::vector<TrackState*> & Tracks,
312 InteractionPoint* IP,
313 Vector3 & Result,
double & ChiSquaredOfFit)
316 this->fitVertex(Tracks, IP, Result, ResultError, ChiSquaredOfFit);
320 void VertexFitterKalman::fitVertex(
const std::vector<TrackState*> & Tracks,
321 InteractionPoint* IP,
322 Vector3 & Result,
double & ChiSquaredOfFit,
323 std::map<TrackState*,double> & ChiSquaredOfTrack,
324 double & ChiSquaredOfIP)
327 this->fitVertex(Tracks, IP, Result, ResultError, ChiSquaredOfFit,
328 ChiSquaredOfTrack, ChiSquaredOfIP);
333 void VertexFitterKalman::fitVertex(
const std::vector<TrackState*> & Tracks,
334 InteractionPoint* IP,
336 double & ChiSquaredOfFit,
337 std::map<TrackState*,double> & ChiSquaredOfTrack,
338 double & ChiSquaredOfIP) {
340 this->fitVertex(Tracks, IP, Result, ResultError, ChiSquaredOfFit);
342 ChiSquaredOfTrack.clear();
343 std::vector<TrackState*>::const_iterator its;
344 for( its = Tracks.begin(); Tracks.end() != its; its++ )
346 TState myState(*its);
347 TState* state = &myState;
348 double chi2 = getDeviationFromVertex( state, fP, fC );
349 ChiSquaredOfTrack.insert( std::pair<TrackState*,double>( (*its), chi2 ) );
353 if( IP ) ChiSquaredOfIP = IP->chi2(Result);
360 double VertexFitterKalman::getDeviationFromVertex(
const TState* state,
362 const double Cv[] )
const
367 double mP[6];
double mC[21];
369 state->TransportBz( state->GetDStoPointBz(v), mP, mC );
371 double d[3]={ v[0]-mP[0], v[1]-mP[1], v[2]-mP[2] };
373 double sigmaS = 0.1 + 10.*sqrt( (d[0]*d[0]+d[1]*d[1]+d[2]*d[2])/
374 (mP[3]*mP[3]+mP[4]*mP[4]+mP[5]*mP[5]) );
376 double h[3] = { mP[3]*sigmaS, mP[4]*sigmaS, mP[5]*sigmaS };
380 mC[1] +h[1]*h[0], mC[2] +h[1]*h[1],
381 mC[3] +h[2]*h[0], mC[4] +h[2]*h[1], mC[5] +h[2]*h[2] };
394 mS[0] = mSi[2]*mSi[5] - mSi[4]*mSi[4];
395 mS[1] = mSi[3]*mSi[4] - mSi[1]*mSi[5];
396 mS[2] = mSi[0]*mSi[5] - mSi[3]*mSi[3];
397 mS[3] = mSi[1]*mSi[4] - mSi[2]*mSi[3];
398 mS[4] = mSi[1]*mSi[3] - mSi[0]*mSi[4];
399 mS[5] = mSi[0]*mSi[2] - mSi[1]*mSi[1];
401 double s = ( mSi[0]*mS[0] + mSi[1]*mS[1] + mSi[3]*mS[3] );
402 s = ( s > 1.E-20 ) ? 1./s : 0;
404 return fabs(s*( ( mS[0]*d[0] + mS[1]*d[1] + mS[3]*d[2])*d[0]
405 +(mS[1]*d[0] + mS[2]*d[1] + mS[4]*d[2])*d[1]
406 +(mS[3]*d[0] + mS[4]*d[1] + mS[5]*d[2])*d[2] ));
409 void VertexFitterKalman::setSeed(
Vector3 Seed)
411 m_useManualSeed =
true;
415 bool VertexFitterKalman::estimateVertex(
double vtx[]) {
416 std::vector<TState>::iterator it1, it2;
417 std::vector<double> vx, vy, vz;
418 for ( it1 = fStates.begin(); fStates.end() != it1+1; it1++ ) {
419 TState* state1 = &(*it1);
420 for ( it2 = it1+1; fStates.end() != it2; it2++ ) {
421 TState* state2 = &(*it2);
423 bool flag = state1->GetDStoTStateBz(state2, ds1, ds2);
424 if( !flag )
continue;
425 double m1[6], m2[6], V1[25], V2[25];
426 state1->TransportBz( ds1, m1, V1 );
427 state2->TransportBz( ds2, m2, V2 );
428 double xg = .5*( m1[0] + m2[0] );
429 double yg = .5*( m1[1] + m2[1] );
430 double zg = .5*( m1[2] + m2[2] );
436 if( vx.size() == 0 )
return false;
437 vtx[0] = robustMean(vx);
438 vtx[1] = robustMean(vy);
439 vtx[2] = robustMean(vz);
443 double VertexFitterKalman::robustMean(std::vector<double> vec)
445 int size = vec.size();
446 if( size == 0 )
return 0;
447 if( size == 1 )
return vec[0];
449 double mean = 0, rms = 0;
450 double rmean = 0, wsum = 0;
452 for(
int i = 0; size != i; i++ ) mean += vec[i];
455 for(
int i = 0; size != i; i++ )
456 rms += (vec[i]-mean)*(vec[i]-mean);
459 for(
int i = 0; size != i; i++ ) {
460 double dif = vec[i]-mean;
461 double weight = exp(-(dif*dif)/(2.0*rms));
462 rmean += weight * vec[i];