Line data Source code
1 : #ifndef EF810BE3_DCD7_4832_94F8_B3F34EDBC3D8
2 : #define EF810BE3_DCD7_4832_94F8_B3F34EDBC3D8
3 :
4 : #include "base/first_include.h" // IWYU pragma: keep
5 : #include "math/linalg/contracts/covariance_matrix_policy_intf.h" // IWYU pragma: keep
6 : #include "math/linalg/covariance_matrix_policies.h" // IWYU pragma: keep
7 : #include "math/linalg/point2d.h"
8 : #include "math/linalg/vector.h"
9 :
10 : namespace tracking
11 : {
12 : namespace env
13 : {
14 :
15 : // TODO(matthias): add interface contract
16 :
17 : /// \brief Ego motion compensation for moving sensor platforms
18 : ///
19 : /// This class implements ego motion compensation for object tracking systems where the sensor
20 : /// platform (typically a vehicle) is moving. It handles the geometric and kinematic transformations
21 : /// required to compensate for the ego vehicle's motion when tracking objects in the environment.
22 : ///
23 : /// The implementation is based on circular motion equations and includes:
24 : /// - Position compensation for objects relative to moving ego vehicle
25 : /// - Direction compensation for velocity vectors
26 : /// - Error propagation for uncertainty in motion parameters using UDU decomposition
27 : /// - Special handling for small angular velocities to avoid numerical issues
28 : /// - Support for both full and factored covariance matrix representations
29 : ///
30 : /// Mathematical Background:
31 : /// - Circular motion with yaw rate ω and speed v for time T
32 : /// - Secant approximation for small angular displacements
33 : /// - First-order error propagation for uncertainty in motion parameters
34 : /// - UDU decomposition for numerical stability in covariance calculations
35 : /// - Specialized equations for ω→0 limit to maintain numerical stability
36 : ///
37 : /// \tparam CovarianceMatrixPolicy_ Policy type that defines the covariance matrix implementation
38 : template <typename CovarianceMatrixPolicy_>
39 : class EgoMotion: public math::contract::CovarianceMatrixPolicyIntf<CovarianceMatrixPolicy_>
40 : {
41 : public:
42 : using value_type = typename CovarianceMatrixPolicy_::value_type;
43 :
44 : /// \brief State indices for displacement vector
45 : ///
46 : /// Defines the order of states in the displacement vector:
47 : /// - DS_X: X-position displacement
48 : /// - DS_Y: Y-position displacement
49 : /// - DS_PSI: Yaw angle displacement (Δψ)
50 : enum DisplacementStates
51 : {
52 : DS_X = 0, ///< X-position displacement index
53 : DS_Y, ///< Y-position displacement index
54 : DS_PSI, ///< Yaw angle displacement index
55 : DS_NUM_VARIABLES ///< Total number of displacement states
56 : };
57 :
58 : /// \brief Displacement vector type
59 : ///
60 : /// Vector containing [Δx, Δy, Δψ] displacements
61 : using DisplacementVec = math::Vector<value_type, DS_NUM_VARIABLES>;
62 :
63 : /// \brief Displacement covariance matrix type
64 : ///
65 : /// Covariance matrix for displacement uncertainties (3×3)
66 : using DisplacementCov = typename CovarianceMatrixPolicy_::template Instantiate<DS_NUM_VARIABLES>;
67 :
68 : /// \brief Inertial Motion parameters structure
69 : ///
70 : /// Contains the inertial motion parameters used in circular motion equations and error propagation.
71 : /// These parameters include:
72 : /// - Velocity (v) and its uncertainty (σ_v)
73 : /// - Acceleration (a) and its uncertainty (σ_a)
74 : /// - Yaw rate (ω) and its uncertainty (σ_ω)
75 : ///
76 : /// These parameters are used to calculate the displacement (dx, dy, dφ) and their covariance
77 : /// using the equations:
78 : /// @f[
79 : /// dφ = ω·T
80 : /// c = \frac{2·v·T + a·T^2}{dφ}·\sin\frac{dφ}{2}
81 : /// dx = c·\cos\frac{dφ}{2}
82 : /// dy = c·\sin\frac{dφ}{2}
83 : /// @f]
84 : ///
85 : /// For small angular velocities (ω→0), simplified equations (linear motion) are used to avoid numerical issues.
86 : struct InertialMotion
87 : {
88 : value_type v{}; ///< Velocity [m/s]
89 : value_type a{}; ///< Acceleration [m/s²]
90 : value_type w{}; ///< Yaw rate [rad/s]
91 : value_type sv{1e-6}; ///< Velocity uncertainty (standard deviation) [m/s]
92 : value_type sa{1e-6}; ///< Acceleration uncertainty (standard deviation) [m/s²]
93 : value_type sw{1e-6}; ///< Yaw rate uncertainty (standard deviation) [rad/s]
94 : };
95 :
96 : /// \brief Vehicle geometry parameters
97 : ///
98 : /// Contains physical dimensions and geometric properties of the ego vehicle
99 : struct Geometry
100 : {
101 : value_type width{}; ///< Vehicle width [m]
102 : value_type length{}; ///< Vehicle length [m]
103 : value_type height{}; ///< Vehicle height [m]
104 :
105 : value_type distCog2Ego{}; ///< Distance from center of gravity to ego reference point [m]
106 : value_type distFrontAxle2Ego{}; ///< Distance from front axle to ego reference point [m]
107 : value_type distFrontAxle2RearAxle{}; ///< Wheelbase (distance between front and rear axles) [m]
108 : };
109 :
110 : /// \brief Displacement information structure
111 : ///
112 : /// Contains displacement vector, covariance, and precomputed trigonometric values
113 : /// for efficient compensation calculations
114 24 : struct Displacement
115 : {
116 : DisplacementVec vec{}; ///< Displacement vector [Δx, Δy, Δψ]
117 : DisplacementCov cov{DisplacementCov::Identity()}; ///< Displacement covariance matrix
118 : value_type sinDeltaPsi{0.0}; ///< sin(Δψ) - precomputed for efficiency
119 : value_type cosDeltaPsi{1.0}; ///< cos(Δψ) - precomputed for efficiency
120 : };
121 :
122 : // rule of 5 declarations
123 : EgoMotion() = delete;
124 : EgoMotion(const EgoMotion&) = default;
125 : EgoMotion(EgoMotion&&) noexcept = default;
126 : auto operator=(const EgoMotion&) -> EgoMotion& = default;
127 : auto operator=(EgoMotion&&) noexcept -> EgoMotion& = default;
128 24 : virtual ~EgoMotion() = default;
129 :
130 : /// \brief Constructor with explicit covariance matrix type
131 : /// \tparam CovarianceMatrixType Template template for covariance matrix type
132 : /// \param motion Motion parameters
133 : /// \param geometry Vehicle geometry
134 : /// \param dt Time interval
135 24 : EgoMotion(const InertialMotion& motion, const Geometry& geometry, const value_type dt)
136 24 : : _motion(motion)
137 24 : , _geometry(geometry)
138 24 : , _dt(dt)
139 : {
140 24 : calcDisplacement();
141 24 : }
142 :
143 : /// \brief Get the time interval for this ego motion compensation
144 : /// \return Time interval Δt [s]
145 : auto getDeltaTime() const -> value_type { return _dt; }
146 :
147 : /// \brief Get the displacement information for center of gravity
148 : /// \return Constant reference to displacement structure
149 : auto getDisplacementCog() const -> const Displacement& { return _displacementCog; }
150 :
151 : /// \brief Get the vehicle geometry parameters
152 : /// \return Constant reference to geometry structure
153 : auto getGeometry() const -> const Geometry& { return _geometry; }
154 :
155 : /// \brief Compensate object position for ego vehicle motion
156 : ///
157 : /// Transforms object positions from old ego coordinate system to new ego coordinate system
158 : /// by applying the inverse of the ego vehicle's motion.
159 : ///
160 : /// \param[out] posXNewEgo X-position in new ego coordinate system [m]
161 : /// \param[out] posYNewEgo Y-position in new ego coordinate system [m]
162 : /// \param[in] posXOldEgo X-position in old ego coordinate system [m]
163 : /// \param[in] posYOldEgo Y-position in old ego coordinate system [m]
164 : void compensatePosition(value_type& posXNewEgo,
165 : value_type& posYNewEgo,
166 : const value_type posXOldEgo,
167 : const value_type posYOldEgo) const;
168 :
169 : /// \brief Compensate direction vector for ego vehicle motion
170 : ///
171 : /// Transforms velocity/direction vectors from old ego coordinate system to new ego coordinate system
172 : /// by applying the inverse of the ego vehicle's rotational motion.
173 : ///
174 : /// \param[out] dxNewEgo X-component of direction in new ego coordinate system
175 : /// \param[out] dyNewEgo Y-component of direction in new ego coordinate system
176 : /// \param[in] dxOldEgo X-component of direction in old ego coordinate system
177 : /// \param[in] dyOldEgo Y-component of direction in old ego coordinate system
178 : void compensateDirection(value_type& dxNewEgo,
179 : value_type& dyNewEgo,
180 : const value_type dxOldEgo,
181 : const value_type dyOldEgo) const;
182 :
183 : /// \brief Get the motion parameters
184 : /// \return Constant reference to motion parameters structure
185 : auto getMotion() const -> const InertialMotion& { return _motion; }
186 :
187 : // clang-format off
188 : TEST_REMOVE_PRIVATE:
189 : ; // workaround for correct indentation
190 : // clang-format on
191 :
192 : [[nodiscard]] auto isLinearMotion() const -> bool;
193 :
194 : /// \brief Compute displacement from internal motion parameters
195 : ///
196 : /// Calculates the displacement (Δx, Δy, Δψ) and covariance using the circular motion equations
197 : // or the linear motion for small angular velocities.
198 : void calcDisplacement();
199 :
200 : /// \brief Calculate Jacobian for linear motion case
201 : ///
202 : /// Computes the Jacobian matrix for linear motion displacement with respect to motion parameters.
203 : ///
204 : /// @param[in] motion Input motion parameters
205 : /// @param[in] dt Time interval for the motion
206 : /// @return Jacobian matrix (3x3)
207 : static auto calcLinearMotionJacobian(const InertialMotion& motion, value_type dt) -> math::SquareMatrix<value_type, 3, true>;
208 :
209 : /// \brief Calculate Jacobian for circular motion case
210 : ///
211 : /// Computes the Jacobian matrix for circular motion displacement with respect to motion parameters.
212 : ///
213 : /// @param[in] motion Input motion parameters
214 : /// @param[in] dt Time interval for the motion
215 : /// @return Jacobian matrix (3x3)
216 : static auto calcCircularMotionJacobian(const InertialMotion& motion, value_type dt) -> math::SquareMatrix<value_type, 3, true>;
217 :
218 : /// \brief Calculate displacement vector
219 : ///
220 : /// Computes the displacement vector [Δx, Δy, Δψ] from motion parameters.
221 : ///
222 : /// @param[out] displacement Output displacement structure
223 : /// @param[in] motion Input motion parameters
224 : /// @param[in] dt Time interval for the motion
225 : static void calcDisplacementVector(Displacement& displacement, const InertialMotion& motion, value_type dt);
226 :
227 : /// \brief Calculate displacement covariance
228 : ///
229 : /// Computes the displacement covariance matrix using the simplified UDU approach.
230 : /// This method uses the generic covariance matrix interface to support both full and factored representations.
231 : ///
232 : /// @param[out] displacement Output displacement structure with computed covariance
233 : /// @param[in] J Jacobian matrix
234 : /// @param[in] motion Input motion parameters
235 : static void calcDisplacementCovariance(Displacement& displacement,
236 : const math::SquareMatrix<value_type, 3, true>& J,
237 : const InertialMotion& motion);
238 :
239 : using Point2d = math::Point2d<value_type>; ///< 2D point type for geometric calculations
240 : InertialMotion _motion{}; ///< Motion parameters (velocity, acceleration, yaw rate and uncertainties)
241 : Geometry _geometry{}; ///< Vehicle geometry parameters
242 : Displacement _displacementCog{}; ///< Displacement information for center of gravity
243 : value_type _dt{}; ///< Time interval for this motion compensation [s]
244 : };
245 :
246 : } // namespace env
247 : } // namespace tracking
248 :
249 : #endif // EF810BE3_DCD7_4832_94F8_B3F34EDBC3D8
|
