29 const unsigned int maximumNumberOfComponents) {
31 const int n = statesToMerge.size();
33 for (
int i = 0;
i <
n; ++
i) {
34 const AmgSymMatrix(5)* measuredCov = statesToMerge[
i].params->covariance();
42 componentsArray.
components[
i].weight = statesToMerge[
i].weight;
47 findMerges(componentsArray, maximumNumberOfComponents);
51 for (
int i = 0;
i < numMerges; ++
i) {
52 const int8_t mini = KL.
merges[
i].To;
53 const int8_t minj = KL.
merges[
i].From;
56 statesToMerge[minj].params.reset();
57 statesToMerge[minj].weight = 0.;
60 for (
auto& state : statesToMerge) {
77 const unsigned int maximumNumberOfComponents) {
80 if (statesToMerge.size() <= maximumNumberOfComponents) {
82 std::move(statesToMerge));
88 bool componentWithoutMeasurement =
false;
89 Trk::MultiComponentState::const_iterator component = statesToMerge.cbegin();
90 for (; component != statesToMerge.cend(); ++component) {
91 if (!component->params->covariance()) {
92 componentWithoutMeasurement =
true;
96 if (componentWithoutMeasurement) {
98 std::sort(statesToMerge.begin(), statesToMerge.end(),
100 return x.weight > y.weight;
105 returnMultiState.push_back(std::move(dummyCompParams));
106 return returnMultiState;
109 return mergeFullDistArray(cache, std::move(statesToMerge),
110 maximumNumberOfComponents);
116 if (uncombinedState.empty()) {
121 uncombinedState.front().params.get();
124 const AmgSymMatrix(5)* firstMeasuredCov = firstParameters->covariance();
126 if (uncombinedState.size() == 1) {
127 return {uncombinedState.front().params->
uniqueClone(),
128 uncombinedState.front().weight};
132 const int dimension = (uncombinedState.front()).
params->parameters().rows();
138 covariancePart1.setZero();
140 covariancePart2.setZero();
143 double totalWeight(0.);
144 for (; component != uncombinedState.
end(); ++component) {
145 double weight = (*component).weight;
149 component = uncombinedState.begin();
151 for (; component != uncombinedState.end(); ++component) {
154 double weight = (*component).weight;
175 const AmgSymMatrix(5)* measuredCov = trackParameters->covariance();
181 covariancePart1 +=
weight * (*measuredCov);
184 Trk::MultiComponentState::const_iterator remainingComponentIterator =
187 for (; remainingComponentIterator != uncombinedState.end();
188 ++remainingComponentIterator) {
190 if (remainingComponentIterator == component) {
197 double remainingComponentIteratorWeight =
198 (*remainingComponentIterator).
weight;
200 covariancePart2 +=
weight * remainingComponentIteratorWeight *
201 parameterDifference *
202 parameterDifference.transpose();
218 covariance = covariancePart1 / sumW + covariancePart2 / (sumW * sumW);
220 if (useMode && dimension == 5) {
223 std::array<double, 10> modes =
234 if (modes[5 + 0] > 0) {
235 double currentErr = sqrt((covariance)(0, 0));
236 currentErr = modes[5 + 0] / currentErr;
237 (covariance)(0, 0) = modes[5 + 0] * modes[5 + 0];
238 covariance.fillSymmetric(1, 0, (covariance)(1, 0) * currentErr);
239 covariance.fillSymmetric(2, 0, (covariance)(2, 0) * currentErr);
240 covariance.fillSymmetric(3, 0, (covariance)(3, 0) * currentErr);
241 covariance.fillSymmetric(4, 0, (covariance)(4, 0) * currentErr);
243 if (modes[5 + 1] > 0) {
244 double currentErr = sqrt((covariance)(1, 1));
245 currentErr = modes[5 + 1] / currentErr;
246 covariance.fillSymmetric(1, 0, (covariance)(1, 0) * currentErr);
247 (covariance)(1, 1) = modes[5 + 1] * modes[5 + 1];
248 covariance.fillSymmetric(2, 1, (covariance)(2, 1) * currentErr);
249 covariance.fillSymmetric(3, 1, (covariance)(3, 1) * currentErr);
250 covariance.fillSymmetric(4, 1, (covariance)(4, 1) * currentErr);
252 if (modes[5 + 2] > 0) {
253 double currentErr = sqrt((covariance)(2, 2));
254 currentErr = modes[5 + 2] / currentErr;
255 covariance.fillSymmetric(2, 0, (covariance)(2, 0) * currentErr);
256 covariance.fillSymmetric(2, 1, (covariance)(2, 1) * currentErr);
257 (covariance)(2, 2) = modes[5 + 2] * modes[5 + 2];
258 covariance.fillSymmetric(3, 2, (covariance)(3, 2) * currentErr);
259 covariance.fillSymmetric(4, 2, (covariance)(4, 2) * currentErr);
261 if (modes[5 + 3] > 0) {
262 double currentErr = sqrt((covariance)(3, 3));
263 currentErr = modes[5 + 3] / currentErr;
264 covariance.fillSymmetric(3, 0, (covariance)(3, 0) * currentErr);
265 covariance.fillSymmetric(3, 1, (covariance)(3, 1) * currentErr);
266 covariance.fillSymmetric(3, 2, (covariance)(3, 2) * currentErr);
267 (covariance)(3, 3) = modes[5 + 3] * modes[5 + 3];
268 covariance.fillSymmetric(4, 3, (covariance)(4, 3) * currentErr);
270 if (modes[5 + 4] > 0) {
271 double currentErr = sqrt((covariance)(4, 4));
272 currentErr = modes[5 + 4] / currentErr;
273 covariance.fillSymmetric(4, 0, (covariance)(4, 0) * currentErr);
274 covariance.fillSymmetric(4, 1, (covariance)(4, 1) * currentErr);
275 covariance.fillSymmetric(4, 2, (covariance)(4, 2) * currentErr);
276 covariance.fillSymmetric(4, 3, (covariance)(4, 3) * currentErr);
277 (covariance)(4, 4) = modes[5 + 4] * modes[5 + 4];
283 std::unique_ptr<Trk::TrackParameters> combinedTrackParameters =
nullptr;
289 if (firstMeasuredCov) {
290 combinedTrackParameters =
294 combinedTrackParameters =
299 return {std::move(combinedTrackParameters), totalWeight};
304 std::unique_ptr<Trk::TrackParameters>
308 combineToSingleImpl(uncombinedState, useMode);
309 return std::move(combinedComponent.
params);
318 const AmgVector(5)& firstParameters = firstTrackParameters->parameters();
319 double firstWeight = mergeTo.
weight;
322 const AmgVector(5)& secondParameters = secondTrackParameters->parameters();
323 double secondWeight = addThis.
weight;
326 AmgVector(5) finalParameters(firstParameters);
327 double finalWeight = firstWeight;
329 finalParameters, finalWeight, secondParameters, secondWeight);
331 const AmgSymMatrix(5)* firstMeasuredCov = firstTrackParameters->covariance();
332 const AmgSymMatrix(5)* secondMeasuredCov = secondTrackParameters->covariance();
334 if (firstMeasuredCov && secondMeasuredCov) {
337 secondParameters, *secondMeasuredCov, secondWeight);
338 mergeTo.
params->updateParameters(finalParameters, finalMeasuredCov);
339 mergeTo.
weight = finalWeight;
341 mergeTo.
params->updateParameters(finalParameters);
342 mergeTo.
weight = finalWeight;
362 const double secondWeight)
364 double totalWeight = firstWeight + secondWeight;
365 double invTotalWeight = 1.0/totalWeight;
366 double deltaPhi = firstParameters[2] - secondParameters[2];
368 firstParameters[2] -= 2 *
M_PI;
370 firstParameters[2] += 2 *
M_PI;
373 (firstWeight * firstParameters + secondWeight * secondParameters) *
377 firstWeight = totalWeight;
388 const double firstWeight,
391 const double secondWeight)
393 double invTotalWeight = 1.0/(firstWeight + secondWeight);
394 AmgVector(5) parameterDifference = firstParameters - secondParameters;
396 parameterDifference *= invTotalWeight;
398 (firstWeight * firstMeasuredCov + secondWeight * secondMeasuredCov) *
400 firstMeasuredCov += firstWeight * secondWeight * parameterDifference *
401 parameterDifference.transpose();
412 unsigned int maximumNumberOfComponents)
415 std::unique_ptr<Trk::MultiComponentState> combinedMultiState =
416 std::make_unique<Trk::MultiComponentState>();
419 for (
const auto& forwardsComponent : forwardsMultiState) {
422 forwardsComponent.params->covariance();
424 for (
const auto& smootherComponent : smootherMultiState) {
427 smootherComponent.params->covariance();
428 if (!smootherMeasuredCov && !forwardMeasuredCov) {
432 if (!forwardMeasuredCov) {
434 smootherComponent.
params->uniqueClone(), smootherComponent.weight};
435 combinedMultiState->push_back(std::move(smootherComponentOnly));
439 if (!smootherMeasuredCov) {
441 forwardsComponent.
params->uniqueClone(), forwardsComponent.weight};
442 combinedMultiState->push_back(std::move(forwardComponentOnly));
447 *forwardMeasuredCov + *smootherMeasuredCov;
449 *forwardMeasuredCov * summedCovariance.inverse();
451 forwardsComponent.params->parameters() +
452 K * (smootherComponent.params->parameters() -
453 forwardsComponent.params->parameters());
455 forwardsComponent.params->parameters() -
456 smootherComponent.params->parameters();
461 std::unique_ptr<Trk::TrackParameters> combinedTrackParameters =
462 (forwardsComponent.params)
463 ->associatedSurface()
464 .createUniqueTrackParameters(newParameters[
Trk::loc1],
469 std::move(covarianceOfNewParameters));
471 summedCovariance.inverse();
475 parametersDiff.transpose() * invertedSummedCovariance * parametersDiff;
476 double weightScalingFactor =
exp(-0.5 * exponent);
477 double combinedWeight = smootherComponent.weight *
478 forwardsComponent.weight * weightScalingFactor;
480 std::move(combinedTrackParameters), combinedWeight};
481 combinedMultiState->push_back(std::move(combinedComponent));
486 merge(std::move(*combinedMultiState), maximumNumberOfComponents);