12 #include <tracking/trackFindingCDC/numerics/JacobianMatrixUtil.h>
14 #include <tracking/trackFindingCDC/numerics/PrecisionMatrix.h>
15 #include <tracking/trackFindingCDC/numerics/JacobianMatrix.h>
16 #include <tracking/trackFindingCDC/numerics/ParameterVector.h>
18 #include <tracking/trackFindingCDC/numerics/EigenView.h>
22 #include <type_traits>
29 namespace TrackFindingCDC {
32 struct PrecisionMatrixUtil {
45 mapToEigen(precision) =
46 mapToEigen(ambiguity).transpose() * mapToEigen(precision) * mapToEigen(ambiguity);
51 template <
int M,
int N>
55 return mapToEigen(ambiguity).transpose() * mapToEigen(precision) * mapToEigen(ambiguity);
75 template <
int N1,
int N2>
80 mapToEigen(result) << mapToEigen(block1), Eigen::Matrix<double, N1, N2>::Zero(),
81 Eigen::Matrix<double, N2, N1>::Zero(), mapToEigen(block2);
86 template <
class APrecisionMatrix,
int I = 0,
int N = 0>
89 constexpr
const int M =
90 std::remove_reference_t<decltype(mapToEigen(APrecisionMatrix()))>::RowsAtCompileTime;
91 return precision.template block<M, M>(I, I);
113 const auto& ePrecision1 = mapToEigen(precision1);
114 const auto& ePrecision2 = mapToEigen(precision2);
115 auto&& ePrecision = mapToEigen(precision);
117 const auto& eParameter1 = mapToEigen(parameter1);
118 const auto& eParameter2 = mapToEigen(parameter2);
119 auto&& eParameter = mapToEigen(parameter);
121 ePrecision = ePrecision1 + ePrecision2;
122 eParameter = ePrecision.colPivHouseholderQr().solve(ePrecision1 * eParameter1 +
123 ePrecision2 * eParameter2);
125 auto eResidual1 = eParameter1 - eParameter;
126 auto eResidual2 = eParameter2 - eParameter;
128 Eigen::Matrix<double, 1, 1> chi2 = (eResidual1.transpose() * ePrecision1 * eResidual1 +
129 eResidual2.transpose() * ePrecision2 * eResidual2);
147 template <
int M,
int N1,
int N2>
157 const auto& eParameter1 = mapToEigen(parameter1);
158 const auto& ePrecision1 = mapToEigen(precision1);
159 const auto& eAmbiguity1 = mapToEigen(ambiguity1);
161 const auto& eParameter2 = mapToEigen(parameter2);
162 const auto& ePrecision2 = mapToEigen(precision2);
163 const auto& eAmbiguity2 = mapToEigen(ambiguity2);
165 auto&& ePrecision = mapToEigen(precision);
166 auto&& eParameter = mapToEigen(parameter);
168 ePrecision = (eAmbiguity1.transpose() * ePrecision1 * eAmbiguity1 +
169 eAmbiguity2.transpose() * ePrecision2 * eAmbiguity2);
171 eParameter = ePrecision.colPivHouseholderQr().solve(
172 eAmbiguity1.transpose() * ePrecision1 * eParameter1 +
173 eAmbiguity2.transpose() * ePrecision2 * eParameter2);
175 auto eResidual1 = eParameter1 - eAmbiguity1 * eParameter;
176 auto eResidual2 = eParameter2 - eAmbiguity2 * eParameter;
178 Eigen::Matrix<double, 1, 1> chi2 = (eResidual1.transpose() * ePrecision1 * eResidual1 +
179 eResidual2.transpose() * ePrecision2 * eResidual2);