58 vector<unsigned short int> notUsedTracks(
fNParticles);
60 notUsedTracks[iTr] = iTr;
62 while(notUsedTracks.size()>0)
64 short int bestTrack = 0;
65 float bestWeight = -1.f;
67 vector<unsigned short int> notUsedTracksNew;
69 for(
unsigned short int iTr = 0; iTr < notUsedTracks.size(); iTr++)
71 unsigned short int &curTrack = notUsedTracks[iTr];
72 float dX =
fParticles[curTrack].CovarianceMatrix()[0];
73 float dY =
fParticles[curTrack].CovarianceMatrix()[2];
74 float dZ =
fParticles[curTrack].CovarianceMatrix()[5];
76 float weight = 1.f/
sqrt(dX + dY + dZ);
77 if (weight > bestWeight)
85 KFParticleCluster cluster;
87 const double *rBest =
fParticles[bestTrack].Parameters();
88 const double *covBest =
fParticles[bestTrack].CovarianceMatrix();
90 float rVertex[3] = {0.f};
91 float covVertex[6] = {0.f};
92 float weightVertex = 0.f;
94 for(
unsigned short int iTr = 0; iTr < notUsedTracks.size(); iTr++)
96 unsigned short int &curTrack = notUsedTracks[iTr];
98 double dr[3] = {rBest[0] -
fParticles[curTrack].X(),
102 double cov[6] = {covBest[0] +
fParticles[curTrack].CovarianceMatrix()[0],
103 covBest[1] +
fParticles[curTrack].CovarianceMatrix()[1],
104 covBest[2] +
fParticles[curTrack].CovarianceMatrix()[2],
105 covBest[3] +
fParticles[curTrack].CovarianceMatrix()[3],
106 covBest[4] +
fParticles[curTrack].CovarianceMatrix()[4],
107 covBest[5] +
fParticles[curTrack].CovarianceMatrix()[5] };
109 float dr2 =
static_cast<float>(dr[0]*dr[0] + dr[1]*dr[1] + dr[2]*dr[2]);
110 float drError2 =
static_cast<float>(
111 dr[0]* (cov[0]* dr[0] + cov[1]* dr[1] + cov[3]* dr[2]) +
112 dr[1]* (cov[1]* dr[0] + cov[2]* dr[1] + cov[4]* dr[2]) +
113 dr[2]* (cov[3]* dr[0] + cov[4]* dr[1] + cov[5]* dr[2])
117 if( dr2*dr2 <= (10*10 * drError2) )
119 float dX =
fParticles[curTrack].CovarianceMatrix()[0];
120 float dY =
fParticles[curTrack].CovarianceMatrix()[2];
121 float dZ =
fParticles[curTrack].CovarianceMatrix()[5];
123 float weight = 1.f/
sqrt(dX + dY + dZ);
125 for(
int iP=0; iP<3; iP++)
126 rVertex[iP] += weight *
fParticles[curTrack].Parameters()[iP];
128 for(
int iC=0; iC<6; iC++)
129 covVertex[iC] += weight * weight *
fParticles[curTrack].CovarianceMatrix()[iC];
131 weightVertex += weight;
132 cluster.fCluster.push_back(curTrack);
151 notUsedTracksNew.push_back(curTrack);
155 notUsedTracks = notUsedTracksNew;
157 if(cluster.fCluster.size() < 2)
continue;
158 if((cluster.fCluster.size() < 3) && (fNParticles>3))
continue;
160 for(
int iP=0; iP<3; iP++)
161 cluster.fP[iP] = rVertex[iP]/weightVertex;
163 for(
int iC=0; iC<6; iC++)
164 cluster.fC[iC] = covVertex[iC]/(weightVertex*weightVertex);
197 static int nVert[10]={0};
200 std::cout <<
"N Vert ";
201 for(
int i=0;
i<10;
i++)
202 std::cout <<
i <<
": " << nVert[
i] <<
" ";
203 std::cout << std::endl;
205 static int nPart[10]={0};
208 std::cout <<
"N Part ";
209 for(
int i=0;
i<10;
i++)
210 std::cout <<
i <<
": " << nPart[
i] <<
" ";
211 std::cout << std::endl;
friend F32vec4 sqrt(const F32vec4 &a)
vector< KFParticleCluster > fClusters
vector< KFParticle > fParticles