Line data Source code
1 : #ifndef FAA51682_4454_40DA_82DD_798B885BD9D8
2 : #define FAA51682_4454_40DA_82DD_798B885BD9D8
3 :
4 : #include "env/ego_motion.h"
5 :
6 : #include "math/analysis/functions.h"
7 : #include "math/linalg/conversions/covariance_matrix_conversions.hpp" // IWYU pragma: keep
8 : #include "math/linalg/covariance_matrix_factored.hpp" // IWYU pragma: keep
9 : #include "math/linalg/covariance_matrix_full.hpp" // IWYU pragma: keep
10 : #include "math/linalg/matrix.hpp" // IWYU pragma: keep
11 : #include "math/linalg/square_matrix.hpp" // IWYU pragma: keep
12 : #include "math/linalg/vector.hpp" // IWYU pragma: keep
13 : #include <cmath>
14 : #include <limits>
15 :
16 : namespace tracking
17 : {
18 : namespace env
19 : {
20 :
21 : template <typename CovarianceMatrixPolicy_>
22 40 : void EgoMotion<CovarianceMatrixPolicy_>::compensatePosition(value_type& posXNewEgo,
23 : value_type& posYNewEgo,
24 : const value_type posXOldEgo,
25 : const value_type posYOldEgo) const
26 : {
27 : // transfer to COG
28 40 : auto posOldCog = Point2d::FromValues(posXOldEgo, posYOldEgo);
29 40 : posOldCog.x() += _geometry.distCog2Ego;
30 :
31 : // translate first
32 : // compensate motion displacement
33 40 : const auto displacement = Point2d::FromValues(_displacementCog.vec.at_unsafe(DS_X), _displacementCog.vec.at_unsafe(DS_Y));
34 40 : const auto translated = Point2d{posOldCog - displacement};
35 :
36 : // rotate according to deltaPsi
37 40 : compensateDirection(posXNewEgo, posYNewEgo, translated.x(), translated.y());
38 :
39 : // transfer from COG
40 40 : posXNewEgo -= _geometry.distCog2Ego;
41 40 : }
42 :
43 : template <typename CovarianceMatrixPolicy_>
44 100 : void EgoMotion<CovarianceMatrixPolicy_>::compensateDirection(value_type& dxNewEgo,
45 : value_type& dyNewEgo,
46 : const value_type dxOldEgo,
47 : const value_type dyOldEgo) const
48 : {
49 : // rotate a vector (velocity or acceleration) according to deltaPsi
50 100 : dxNewEgo = (_displacementCog.cosDeltaPsi * dxOldEgo) + (_displacementCog.sinDeltaPsi * dyOldEgo);
51 100 : dyNewEgo = -(_displacementCog.sinDeltaPsi * dxOldEgo) + (_displacementCog.cosDeltaPsi * dyOldEgo);
52 100 : }
53 :
54 : template <typename CovarianceMatrixPolicy_>
55 24 : auto EgoMotion<CovarianceMatrixPolicy_>::isLinearMotion() const -> bool
56 : {
57 24 : constexpr value_type omegaThreshold = static_cast<value_type>(9e-3);
58 24 : return (math::pow<4>(_motion.w) < omegaThreshold);
59 : }
60 :
61 : template <typename CovarianceMatrixPolicy_>
62 24 : void EgoMotion<CovarianceMatrixPolicy_>::calcDisplacement()
63 : {
64 24 : calcDisplacementVector(_displacementCog, _motion, _dt);
65 24 : const auto J = isLinearMotion() ? calcLinearMotionJacobian(_motion, _dt) : calcCircularMotionJacobian(_motion, _dt);
66 24 : calcDisplacementCovariance(_displacementCog, J, _motion);
67 24 : }
68 :
69 : template <typename CovarianceMatrixPolicy_>
70 20 : auto EgoMotion<CovarianceMatrixPolicy_>::calcLinearMotionJacobian(const InertialMotion& motion,
71 : value_type dt) -> math::SquareMatrix<value_type, 3, true>
72 : {
73 20 : const auto T = dt;
74 20 : const auto v = motion.v;
75 20 : const auto a = motion.a;
76 :
77 20 : math::SquareMatrix<value_type, 3, true> J{};
78 20 : J.setZeros();
79 20 : J.at_unsafe(0, 0) = T;
80 20 : J.at_unsafe(0, 1) = (0.5) * pow(T, 2);
81 20 : J.at_unsafe(1, 2) = (0.25) * pow(T, 3) * a + (0.5) * pow(T, 2) * v;
82 20 : J.at_unsafe(2, 2) = T;
83 :
84 20 : return J;
85 : }
86 :
87 : template <typename CovarianceMatrixPolicy_>
88 4 : auto EgoMotion<CovarianceMatrixPolicy_>::calcCircularMotionJacobian(const InertialMotion& motion,
89 : value_type dt) -> math::SquareMatrix<value_type, 3, true>
90 : {
91 4 : const auto T = dt;
92 4 : const auto v = motion.v;
93 4 : const auto a = motion.a;
94 4 : const auto w = motion.w;
95 :
96 4 : math::SquareMatrix<value_type, 3, true> J{};
97 4 : J.setZeros();
98 4 : J.at_unsafe(0, 0) = 2 * std::sin((0.5) * T * w) * std::cos((0.5) * T * w) / w;
99 4 : J.at_unsafe(0, 1) = T * std::sin((0.5) * T * w) * std::cos((0.5) * T * w) / w;
100 4 : J.at_unsafe(0, 2) = -0.5 * T * (T * a + 2 * v) * math::pow<2>(std::sin((0.5) * T * w)) / w +
101 4 : (0.5) * T * (T * a + 2 * v) * math::pow<2>(std::cos((0.5) * T * w)) / w -
102 4 : (T * a + 2 * v) * std::sin((0.5) * T * w) * std::cos((0.5) * T * w) / (w * w);
103 8 : J.at_unsafe(1, 0) = 2 * math::pow<2>(std::sin((0.5) * T * w)) / w;
104 8 : J.at_unsafe(1, 1) = T * math::pow<2>(std::sin((0.5) * T * w)) / w;
105 8 : J.at_unsafe(1, 2) = T * (T * a + 2 * v) * std::sin((0.5) * T * w) * std::cos((0.5) * T * w) / w -
106 4 : (T * a + 2 * v) * math::pow<2>(std::sin((0.5) * T * w)) / (w * w);
107 4 : J.at_unsafe(2, 2) = T;
108 :
109 4 : return J;
110 : }
111 :
112 : template <typename CovarianceMatrixPolicy_>
113 24 : void EgoMotion<CovarianceMatrixPolicy_>::calcDisplacementVector(Displacement& displacement,
114 : const InertialMotion& motion,
115 : value_type dt)
116 : {
117 24 : const value_type T = dt;
118 24 : const value_type v = motion.v;
119 24 : const value_type a = motion.a;
120 24 : const value_type w = motion.w;
121 24 : if (std::abs(T * w) < std::numeric_limits<value_type>::epsilon())
122 : {
123 : // special case for (T*w) very close to zero
124 8 : displacement.vec.setZeros();
125 8 : displacement.vec.at_unsafe(DS_X) = 0.5 * T * (2 * v + a * T);
126 8 : displacement.sinDeltaPsi = static_cast<value_type>(0);
127 8 : displacement.cosDeltaPsi = static_cast<value_type>(1);
128 : }
129 : else
130 : {
131 : // Calculate angular displacement: dφ = ω·T
132 16 : const value_type dphi = w * dt;
133 16 : const value_type dphi_2 = dphi / 2.0;
134 : // Precompute trigonometric values for efficiency
135 16 : const value_type sin_dphi = std::sin(dphi);
136 16 : const value_type sin_dphi_2 = std::sin(dphi_2);
137 16 : const value_type cos_dphi = std::cos(dphi);
138 16 : const value_type cos_dphi_2 = std::cos(dphi_2);
139 :
140 : // Calculate secant length
141 : // c = (2·v·T + a·T^2)/dφ · sin(dφ/2) = T * (2·v + a·T)/dφ · sin(dφ/2)
142 16 : const value_type c = T * (2.0 * v + a * T) / dphi * sin_dphi_2;
143 :
144 : // Calculate displacement components
145 16 : const value_type dx = c * cos_dphi_2;
146 16 : const value_type dy = c * sin_dphi_2;
147 :
148 : // Set displacement vector
149 16 : displacement.vec.at_unsafe(DS_X) = dx;
150 16 : displacement.vec.at_unsafe(DS_Y) = dy;
151 16 : displacement.vec.at_unsafe(DS_PSI) = dphi;
152 16 : displacement.sinDeltaPsi = sin_dphi;
153 16 : displacement.cosDeltaPsi = cos_dphi;
154 : }
155 24 : }
156 :
157 : template <typename CovarianceMatrixPolicy_>
158 24 : void EgoMotion<CovarianceMatrixPolicy_>::calcDisplacementCovariance(Displacement& displacement,
159 : const math::SquareMatrix<value_type, 3, true>& J,
160 : const InertialMotion& motion)
161 : {
162 : // clang-format off
163 : // Create diagonal matrix from motion uncertainties
164 96 : auto diag =
165 : math::DiagonalMatrix<value_type, 3>::FromList({
166 96 : math::pow<2>(motion.sv), math::pow<2>(motion.sa), math::pow<2>(motion.sw)
167 : });
168 : // clang-format on
169 :
170 : // Create covariance matrix using generic FromDiagonal constructor
171 24 : displacement.cov = std::move(DisplacementCov::FromDiagonal(diag));
172 :
173 : // Apply the transformation: P = J * Pin * J^T using apaT
174 24 : displacement.cov.apaT(J);
175 24 : }
176 :
177 : } // namespace env
178 : } // namespace tracking
179 :
180 : #endif // FAA51682_4454_40DA_82DD_798B885BD9D8
|
