10 #include <tracking/trackFindingCDC/fitting/EigenObservationMatrix.h>
12 #include <tracking/trackFindingCDC/fitting/CDCObservations2D.h>
13 #include <tracking/trackFindingCDC/fitting/CDCSZObservations.h>
18 using namespace TrackFindingCDC;
20 TrackFindingCDC::EigenObservationMatrix
21 TrackFindingCDC::getEigenObservationMatrix(CDCObservations2D* observations2D)
23 std::size_t nObservations = observations2D->size();
24 double* rawObservations = observations2D->data();
25 EigenObservationMatrix eigenObservations(rawObservations, nObservations, 4);
26 return eigenObservations;
29 TrackFindingCDC::EigenObservationMatrix
30 TrackFindingCDC::getEigenObservationMatrix(CDCSZObservations* szObservations)
32 std::size_t nObservations = szObservations->size();
33 double* rawObservations = szObservations->data();
34 EigenObservationMatrix eigenObservations(rawObservations, nObservations, 3);
35 return eigenObservations;
38 Eigen::Matrix<double, 5, 5> TrackFindingCDC::getWXYRLSumMatrix(CDCObservations2D& observations2D)
40 EigenObservationMatrix eigenObservations = getEigenObservationMatrix(&observations2D);
41 assert(eigenObservations.cols() == 4);
42 std::size_t nObservations = eigenObservations.rows();
46 Eigen::Matrix<double, Eigen::Dynamic, 5> projectedPoints(nObservations, 5);
48 const std::size_t iW = 0;
49 const std::size_t iX = 1;
50 const std::size_t iY = 2;
51 const std::size_t iR2 = 3;
52 const std::size_t iL = 4;
54 projectedPoints.col(iW) =
55 Eigen::Matrix<double, Eigen::Dynamic, 1>::Constant(nObservations, 1.0);
56 projectedPoints.col(iX) = eigenObservations.col(0);
57 projectedPoints.col(iY) = eigenObservations.col(1);
58 projectedPoints.col(iR2) = eigenObservations.leftCols<2>().rowwise().squaredNorm() -
59 eigenObservations.col(2).rowwise().squaredNorm();
60 projectedPoints.col(iL) = eigenObservations.col(2);
62 Eigen::Array<double, Eigen::Dynamic, 1> weights = eigenObservations.col(3);
63 Eigen::Matrix<double, Eigen::Dynamic, 5> weightedProjectedPoints =
64 projectedPoints.array().colwise() * weights;
65 Eigen::Matrix<double, 5, 5> sumMatrix = weightedProjectedPoints.transpose() * projectedPoints;
72 Eigen::Matrix<double, 4, 4> TrackFindingCDC::getWXYLSumMatrix(CDCObservations2D& observations2D)
74 EigenObservationMatrix eigenObservations = getEigenObservationMatrix(&observations2D);
75 assert(eigenObservations.cols() == 4);
76 std::size_t nObservations = eigenObservations.rows();
78 Eigen::Matrix<double, Eigen::Dynamic, 4> projectedPoints(nObservations, 4);
80 const std::size_t iW = 0;
81 const std::size_t iX = 1;
82 const std::size_t iY = 2;
83 const std::size_t iL = 3;
85 projectedPoints.col(iW) =
86 Eigen::Matrix<double, Eigen::Dynamic, 1>::Constant(nObservations, 1.0);
87 projectedPoints.col(iX) = eigenObservations.col(0);
88 projectedPoints.col(iY) = eigenObservations.col(1);
89 projectedPoints.col(iL) = eigenObservations.col(2);
91 Eigen::Array<double, Eigen::Dynamic, 1> weights = eigenObservations.col(3);
92 Eigen::Matrix<double, Eigen::Dynamic, 4> weightedProjectedPoints =
93 projectedPoints.array().colwise() * weights;
94 Eigen::Matrix<double, 4, 4> sumMatrix = weightedProjectedPoints.transpose() * projectedPoints;
99 Eigen::Matrix<double, 4, 4> TrackFindingCDC::getWXYRSumMatrix(CDCObservations2D& observations2D)
101 EigenObservationMatrix eigenObservations = getEigenObservationMatrix(&observations2D);
102 assert(eigenObservations.cols() == 4);
103 std::size_t nObservations = eigenObservations.rows();
105 Eigen::Matrix<double, Eigen::Dynamic, 4> projectedPoints(nObservations, 4);
107 const std::size_t iW = 0;
108 const std::size_t iX = 1;
109 const std::size_t iY = 2;
110 const std::size_t iR2 = 3;
112 projectedPoints.col(iW) =
113 Eigen::Matrix<double, Eigen::Dynamic, 1>::Constant(nObservations, 1.0);
114 projectedPoints.col(iX) = eigenObservations.col(0);
115 projectedPoints.col(iY) = eigenObservations.col(1);
116 projectedPoints.col(iR2) = eigenObservations.leftCols<2>().rowwise().squaredNorm() -
117 eigenObservations.col(2).rowwise().squaredNorm();
119 Eigen::Array<double, Eigen::Dynamic, 1> weights = eigenObservations.col(3);
120 Eigen::Matrix<double, Eigen::Dynamic, 4> weightedProjectedPoints =
121 projectedPoints.array().colwise() * weights;
122 Eigen::Matrix<double, 4, 4> sumMatrix = weightedProjectedPoints.transpose() * projectedPoints;
126 Eigen::Matrix<double, 3, 3> TrackFindingCDC::getWXYSumMatrix(CDCObservations2D& observations2D)
128 EigenObservationMatrix eigenObservations = getEigenObservationMatrix(&observations2D);
129 assert(eigenObservations.cols() == 4);
130 std::size_t nObservations = eigenObservations.rows();
132 Eigen::Matrix<double, Eigen::Dynamic, 3> projectedPoints(nObservations, 3);
134 const std::size_t iW = 0;
135 const std::size_t iX = 1;
136 const std::size_t iY = 2;
138 projectedPoints.col(iW) =
139 Eigen::Matrix<double, Eigen::Dynamic, 1>::Constant(nObservations, 1.0);
140 projectedPoints.col(iX) = eigenObservations.col(0);
141 projectedPoints.col(iY) = eigenObservations.col(1);
143 Eigen::Array<double, Eigen::Dynamic, 1> weights = eigenObservations.col(3);
144 Eigen::Matrix<double, Eigen::Dynamic, 3> weightedProjectedPoints =
145 projectedPoints.array().colwise() * weights;
146 Eigen::Matrix<double, 3, 3> sumMatrix = weightedProjectedPoints.transpose() * projectedPoints;
151 Eigen::Matrix<double, 3, 3> TrackFindingCDC::getWSZSumMatrix(CDCSZObservations& szObservations)
153 EigenObservationMatrix eigenObservations = getEigenObservationMatrix(&szObservations);
154 assert(eigenObservations.cols() == 3);
155 std::size_t nObservations = eigenObservations.rows();
157 Eigen::Matrix<double, Eigen::Dynamic, 3> projectedPoints(nObservations, 3);
159 const std::size_t iW = 0;
160 const std::size_t iS = 1;
161 const std::size_t iZ = 2;
163 projectedPoints.col(iW) = Eigen::Matrix<double, Eigen::Dynamic, 1>::Constant(nObservations, 1.0);
164 projectedPoints.col(iS) = eigenObservations.col(0);
165 projectedPoints.col(iZ) = eigenObservations.col(1);
167 Eigen::Array<double, Eigen::Dynamic, 1> weights = eigenObservations.col(2);
168 Eigen::Matrix<double, Eigen::Dynamic, 3> weightedProjectedPoints = projectedPoints.array().colwise() * weights;
169 Eigen::Matrix<double, 3, 3> sumMatrix = weightedProjectedPoints.transpose() * projectedPoints;