Line data Source code
1 : #ifndef D33C0BB9_EF21_44C6_8DAD_0C38C418D824
2 : #define D33C0BB9_EF21_44C6_8DAD_0C38C418D824
3 :
4 : #include "base/first_include.h" // IWYU pragma: keep
5 : #include "base/require_abstract_intf.h"
6 : #include "env/ego_motion.h"
7 : #include "filter/information_filter.h"
8 : #include "filter/kalman_filter.h"
9 : #include "math/linalg/contracts/covariance_matrix_policy_intf.h" // IWYU pragma: keep
10 : #include "math/linalg/conversions/covariance_matrix_conversions.hpp"
11 : #include "math/linalg/errors.h"
12 : #include "motion/generic_predict.hpp" // IWYU pragma: keep
13 : #include "motion/motion_model_traits.h" // IWYU pragma: keep
14 : #include "motion/state_mem.h"
15 :
16 : namespace tracking
17 : {
18 : namespace motion
19 : {
20 :
21 : // TODO(matthias): add interface contract
22 : // TODO(matthias): add doxygen
23 :
24 : /// \brief Abstract Motion Model interface
25 : /// \tparam CovarianceMatrixPolicy_ Policy type that defines the covariance matrix implementation
26 : template <typename CovarianceMatrixPolicy_>
27 : class IMotionModel
28 : : public math::contract::CovarianceMatrixPolicyIntf<CovarianceMatrixPolicy_>
29 : , public base::contract::RequireAbstractIntf<IMotionModel<CovarianceMatrixPolicy_>>
30 : {
31 : public:
32 : using value_type = typename CovarianceMatrixPolicy_::value_type;
33 : using EgoMotionType = env::EgoMotion<CovarianceMatrixPolicy_>;
34 : using KalmanFilterType = filter::KalmanFilter<CovarianceMatrixPolicy_>;
35 : using InformationFilterType = filter::InformationFilter<CovarianceMatrixPolicy_>;
36 :
37 : // rule of 5 declarations
38 24 : IMotionModel() = default;
39 12 : virtual ~IMotionModel() = default;
40 :
41 : virtual auto getX() const -> value_type = 0;
42 : virtual auto getVx() const -> value_type = 0;
43 : virtual auto getAx() const -> value_type = 0;
44 : virtual auto getY() const -> value_type = 0;
45 : virtual auto getVy() const -> value_type = 0;
46 : virtual auto getAy() const -> value_type = 0;
47 :
48 : /// \brief Predicts the underlying MotionModel with the given filter (includes ego motion compensation)
49 : /// \param[in] dt The delta time from last state to predicted state
50 : /// \param[in] filter The filter instance
51 : /// \param[in] egoMotion The known egoMotion from last state to predicted state
52 : virtual void predict(const value_type dt, const KalmanFilterType& filter, const EgoMotionType& egoMotion) = 0;
53 :
54 : /// \brief Predicts the underlying MotionModel with the given filter (includes ego motion compensation)
55 : /// \param[in] dt The delta time from last state to predicted state
56 : /// \param[in] filter The filter instance
57 : /// \param[in] egoMotion The known egoMotion from last state to predicted state
58 : virtual void predict(const value_type dt, const InformationFilterType& filter, const EgoMotionType& egoMotion) = 0;
59 :
60 : // clang-format off
61 : TEST_REMOVE_PROTECTED:
62 : ; // workaround to keep following idententation
63 : // clang-format on
64 :
65 : // rule of 5 declarations (remaining declarations are protected according to A12-8-6)
66 : IMotionModel(const IMotionModel& other) = default;
67 : IMotionModel(IMotionModel&&) noexcept = default;
68 : auto operator=(const IMotionModel& other) -> IMotionModel& = default;
69 : auto operator=(IMotionModel&&) noexcept -> IMotionModel& = default;
70 : };
71 :
72 : /// \brief Abstract MotionModel with known dimension, keeping the model memory for State and StateCovariance
73 : /// \tparam MotionModel_
74 : /// \tparam MotionModelTrait_
75 : template <typename MotionModel_, typename MotionModelTrait_>
76 : class ExtendedMotionModel
77 : : public IMotionModel<typename MotionModelTrait_::CovarianceMatrixPolicy>
78 : , public generic::Predict<MotionModel_, typename MotionModelTrait_::CovarianceMatrixPolicy>
79 : , public StateMem<typename MotionModelTrait_::CovarianceMatrixPolicy, MotionModelTrait_::Size>
80 : , public base::contract::RequireAbstractIntf<ExtendedMotionModel<MotionModel_, MotionModelTrait_>>
81 : {
82 : public:
83 : using value_type = typename MotionModelTrait_::value_type;
84 : using StateDef = typename MotionModelTrait_::StateDef;
85 : using CovarianceMatrixPolicy = typename MotionModelTrait_::CovarianceMatrixPolicy;
86 : using BaseIMotionModel = IMotionModel<CovarianceMatrixPolicy>;
87 : using EgoMotionType = typename BaseIMotionModel::EgoMotionType;
88 : using KalmanFilterType = typename BaseIMotionModel::KalmanFilterType;
89 : using InformationFilterType = typename BaseIMotionModel::InformationFilterType;
90 : using BaseGenericPredict = generic::Predict<MotionModel_, typename MotionModelTrait_::CovarianceMatrixPolicy>;
91 : using BaseStateMem = StateMem<CovarianceMatrixPolicy, MotionModelTrait_::Size>;
92 : using typename BaseStateMem::StateCov;
93 : using typename BaseStateMem::StateVec;
94 :
95 : // rule of 5 declarations
96 4 : ExtendedMotionModel() = default;
97 12 : virtual ~ExtendedMotionModel() = default;
98 :
99 : /// \brief Create state vector from initializer list
100 : /// \param[in] list Initializer list with state values
101 : /// \return StateVec
102 36 : static auto StateVecFromList(const std::initializer_list<value_type>& list) -> StateVec { return StateVec::FromList(list); }
103 :
104 : /// \brief Create state covariance from initializer list
105 : /// \param[in] list Nested initializer list with covariance values
106 : /// \return StateCov
107 36 : static auto StateCovFromList(const std::initializer_list<std::initializer_list<value_type>>& list) -> StateCov
108 : {
109 : if constexpr (CovarianceMatrixPolicy::is_factored)
110 : {
111 18 : return math::conversions::CovarianceMatrixFactoredFromList<value_type, MotionModelTrait_::Size>(list);
112 : }
113 : else
114 : {
115 18 : return StateCov::FromList(list);
116 : }
117 : }
118 :
119 : /// \brief Create factored state covariance from initializer list
120 : /// \param[in] u Nested initializer list for the upper triangular U matrix
121 : /// \param[in] d Flat initializer list for the diagonal D matrix
122 : /// \return StateCov
123 : template <typename T = CovarianceMatrixPolicy>
124 : static auto StateCovFromList(const std::initializer_list<std::initializer_list<value_type>>& u,
125 : const std::initializer_list<value_type>& d) -> std::enable_if_t<T::is_factored, StateCov>
126 : {
127 : return StateCov::FromList(u, d);
128 : }
129 :
130 : /// \brief Create complete ExtendedMotionModel from initializer lists
131 : /// \param[in] vecList Initializer list for state vector
132 : /// \param[in] covList Nested initializer list for covariance matrix
133 : /// \return ExtendedMotionModel instance
134 : static auto FromLists(const std::initializer_list<value_type>& vecList,
135 : const std::initializer_list<std::initializer_list<value_type>>& covList) -> MotionModel_
136 : {
137 : auto vec = StateVecFromList(vecList);
138 : auto cov = StateCovFromList(covList);
139 : return MotionModel_{vec, cov};
140 : }
141 :
142 : /// \brief Read access to x position
143 : /// \return value_type
144 0 : auto getX() const -> value_type final { return this->operator[](StateDef::X); }
145 :
146 : /// \brief Read access to y position
147 : /// \return value_type
148 0 : auto getY() const -> value_type final { return this->operator[](StateDef::Y); }
149 :
150 : /// \brief Inverts the state covariance matrix into information form and vice versa
151 : auto invertCov() -> tl::expected<void, math::Errors>;
152 :
153 : /// \brief Predicts the underlying MotionModel with the given filter (includes ego motion compensation)
154 : /// \param[in] dt The delta time from last state to predicted state
155 : /// \param[in] filter The kalman filter instance
156 : /// \param[in] egoMotion The known egoMotion from last state to predicted state
157 40 : void predict(const value_type dt, const KalmanFilterType& filter, const EgoMotionType& egoMotion) final
158 : {
159 40 : BaseGenericPredict::run(dt, filter, egoMotion);
160 40 : }
161 :
162 : /// \brief Predicts the underlying MotionModel with the given filter (includes ego motion compensation)
163 : /// \param[in] dt The delta time from last state to predicted state
164 : /// \param[in] filter The information filter instance
165 : /// \param[in] egoMotion The known egoMotion from last state to predicted state
166 40 : void predict(const value_type dt, const InformationFilterType& filter, const EgoMotionType& egoMotion) final
167 : {
168 40 : BaseGenericPredict::run(dt, filter, egoMotion);
169 40 : }
170 :
171 : /// \brief Transform information space into state space
172 40 : void convertStateVecIntoStateSpace()
173 : {
174 40 : const auto& y = this->getVec();
175 40 : const auto& Y = this->getCovForInternalUse();
176 40 : auto& x = this->getVecForInternalUse();
177 : // precondition: y and Y must be in information space (Y = P^-1, y = Y * x)
178 : // operation:
179 : // Y * result = y
180 : // result = Y^-1 * y = Y^-1 * Y * x = x
181 40 : x = Y().qrSolve(y);
182 : // postcondition: x is in state space, Y is unchanged (still in information space)
183 40 : }
184 :
185 : /// \brief Transform state space into information space
186 40 : void convertStateVecIntoInformationSpace()
187 : {
188 40 : const auto& x = this->getVec();
189 40 : const auto& Y = this->getCovForInternalUse();
190 40 : auto& y = this->getVecForInternalUse();
191 : // precondition: x must be in state space, Y must be in information space (Y = P^-1)
192 : // operation:
193 : // y = Y * x
194 40 : y = static_cast<StateVec>(Y() * x);
195 : // postcondition: y is in information space (y = Y * x), Y is unchanged (still in information space)
196 40 : }
197 :
198 : // clang-format off
199 : TEST_REMOVE_PROTECTED:
200 : ; // workaround to keep following idententation
201 : // clang-format on
202 :
203 : // rule of 5 declarations (remaining declarations are protected according to A12-8-6)
204 : ExtendedMotionModel(const ExtendedMotionModel& other) = default;
205 : ExtendedMotionModel(ExtendedMotionModel&&) noexcept = default;
206 : auto operator=(const ExtendedMotionModel& other) -> ExtendedMotionModel& = default;
207 : auto operator=(ExtendedMotionModel&&) noexcept -> ExtendedMotionModel& = default;
208 :
209 : /// \brief Testing: Construct a new Extended Motion Model object
210 : /// \param[in] vec
211 : /// \param[in] cov
212 20 : explicit ExtendedMotionModel(const StateVec& vec, const StateCov& cov)
213 : : BaseIMotionModel{}
214 20 : , BaseStateMem{vec, cov}
215 : {
216 20 : assert(cov.determinant() > 0);
217 20 : }
218 : };
219 :
220 : template <typename MotionModel_, typename MotionModelTrait_>
221 : auto ExtendedMotionModel<MotionModel_, MotionModelTrait_>::invertCov() -> tl::expected<void, math::Errors>
222 : {
223 : auto&& res = this->getCov().inverse();
224 : if (res.has_value())
225 : {
226 : this->getCovForInternalUse() = res.value();
227 : return {};
228 : }
229 : else
230 : {
231 : return tl::unexpected<math::Errors>{res.error()};
232 : }
233 : }
234 :
235 : } // namespace motion
236 : } // namespace tracking
237 :
238 : #endif // D33C0BB9_EF21_44C6_8DAD_0C38C418D824
|
