Branch data Line data Source code
1 : : // Copyright 2019 the Autoware Foundation
2 : : //
3 : : // Licensed under the Apache License, Version 2.0 (the "License");
4 : : // you may not use this file except in compliance with the License.
5 : : // You may obtain a copy of the License at
6 : : //
7 : : // http://www.apache.org/licenses/LICENSE-2.0
8 : : //
9 : : // Unless required by applicable law or agreed to in writing, software
10 : : // distributed under the License is distributed on an "AS IS" BASIS,
11 : : // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12 : : // See the License for the specific language governing permissions and
13 : : // limitations under the License.
14 : : //
15 : : // Co-developed by Tier IV, Inc. and Apex.AI, Inc.
16 : :
17 : : #ifndef OPTIMIZATION__OPTIMIZATION_PROBLEM_HPP_
18 : : #define OPTIMIZATION__OPTIMIZATION_PROBLEM_HPP_
19 : :
20 : : #include <common/types.hpp>
21 : : #include <optimization/visibility_control.hpp>
22 : : #include <optimization/utils.hpp>
23 : : #include <helper_functions/crtp.hpp>
24 : : #include <Eigen/Core>
25 : : #include <vector>
26 : : #include <cstddef>
27 : : #include <memory>
28 : : #include <tuple>
29 : : #include <utility>
30 : :
31 : : using autoware::common::types::bool8_t;
32 : :
33 : : namespace autoware
34 : : {
35 : : namespace common
36 : : {
37 : : namespace optimization
38 : : {
39 : : /// CRTP base class for a mathematical expression.
40 : : /// \tparam Derived Implementation class. Must implement `score_(..)`, `jacobian_()` and
41 : : /// `hessian(..)`.
42 : : /// \tparam DomainValueT Parameter type.
43 : : /// \tparam NumJacobianColsT Number of columns in the jacobian matrix.
44 : : /// \tparam NumVarsT Number of variables.
45 : : template<typename Derived, typename DomainValueT, int NumJacobianColsT, int NumVarsT>
46 : : class OPTIMIZATION_PUBLIC Expression : public common::helper_functions::crtp<Derived>
47 : : {
48 : : public:
49 : : static constexpr auto NumJacobianCols = NumJacobianColsT;
50 : : static constexpr auto NumVars = NumVarsT;
51 : : using DomainValue = DomainValueT;
52 : : using Value = autoware::common::types::float64_t;
53 : : using Jacobian = Eigen::Matrix<Value, NumVars, NumJacobianCols>;
54 : : using Hessian = Eigen::Matrix<Value, NumVars, NumVars>;
55 : : using JacobianRef = Eigen::Ref<Jacobian>;
56 : : using HessianRef = Eigen::Ref<Hessian>;
57 : :
58 : : /// Get the result of an expression for a given parameter value.
59 : : /// \param x Parameter value
60 : : /// \return Evaluated score
61 : : Value operator()(const DomainValue & x)
62 : : {
63 : : return this->impl().score_(x);
64 : : }
65 : :
66 : : /// Get the jacobian at a given parameter value.
67 : : /// \param x Parameter value.
68 : : /// \param out Evaluated jacobian matrix.
69 : 125 : void jacobian(const DomainValue & x, JacobianRef out)
70 : : {
71 [ + + - ]: 291 : this->impl().jacobian_(x, out);
72 : 125 : }
73 : :
74 : : /// Get the hessian at a given parameter value.
75 : : /// \param x Parameter value.{
76 : : /// \param out Evaluated hessian matrix.
77 : 125 : void hessian(const DomainValue & x, HessianRef out)
78 : : {
79 : 125 : this->impl().hessian_(x, out);
80 : 125 : }
81 : :
82 : : /// Pre-compute and cache the score/jacobian/hessian values. Which terms are to be computed is
83 : : /// specified with the `ComputeMode` argument. By default, this function does nothing. You can
84 : : /// implement and hide this function in your implementation to allow for some caching behavior.
85 : : /// Though it is encouraged to use `CachedExpression` which already handles a big portion of the
86 : : /// cache state management.
87 : : /// \param x Parameter value.
88 : : /// \param mode Computation mode. Which terms (score, jacobian, hessian) are to be computed
89 : : /// can be set within this element.
90 : : void evaluate(const DomainValue & x, const ComputeMode & mode)
91 : : {
92 : : (void)x;
93 : : (void)mode;
94 : : }
95 : : };
96 : :
97 : : /// Expression class for cases when pre-evaluating elements or computing elements together
98 : : /// may be more efficient. This class implements the necessary boilerplate to manage the cache
99 : : /// state book-keeping.
100 : : /// This class implements `score_(..)`, `jacobian_(..)` and `hessian_(..)`; hence the
101 : : /// implementation must only implement `evaluate_(..)`
102 : : /// \tparam Derived CRTP implementation class. This class should implement `evaluate_()`.
103 : : /// \tparam DomainValueT Parameter type.
104 : : /// \tparam NumJacobianColsT Number of columns in the jacobian matrix.
105 : : /// \tparam NumVarsT Number of variables.
106 : : /// \tparam ComparatorT Comparator function for `DomainValueT`
107 : : template<typename Derived, typename DomainValueT, int NumJacobianColsT, int NumVarsT,
108 : : typename ComparatorT>
109 : : class CachedExpression : public
110 : : Expression<CachedExpression<Derived, DomainValueT,
111 : : NumJacobianColsT, NumVarsT, ComparatorT>,
112 : : DomainValueT, NumJacobianColsT, NumVarsT>
113 : : {
114 : : public:
115 : : EIGEN_MAKE_ALIGNED_OPERATOR_NEW
116 : : static constexpr auto NumJacobianCols = NumJacobianColsT;
117 : : static constexpr auto NumVars = NumVarsT;
118 : : using DomainValue = DomainValueT;
119 : : using Value = autoware::common::types::float64_t;
120 : : using Jacobian = Eigen::Matrix<Value, NumVars, NumJacobianCols>;
121 : : using Hessian = Eigen::Matrix<Value, NumVars, NumVars>;
122 : : using JacobianRef = Eigen::Ref<Eigen::Matrix<Value, NumVars, NumJacobianCols>>;
123 : : using HessianRef = Eigen::Ref<Eigen::Matrix<Value, NumVars, NumVars>>;
124 : :
125 : : explicit CachedExpression(const ComparatorT & comparator = ComparatorT())
126 : : : m_score{0.0}, m_jacobian(Jacobian::Zero()), m_hessian{Hessian::Zero()},
127 : : m_cache_state(comparator) {}
128 : :
129 : : Value score_(const DomainValue & x)
130 : : {
131 : : if (!m_cache_state.is_cached(x, common::optimization::ExpressionTerm::SCORE)) {
132 : : evaluate(x, ComputeMode{}.set_score());
133 : : }
134 : : return m_score;
135 : : }
136 : :
137 : : void jacobian_(const DomainValue & x, JacobianRef out)
138 : : {
139 : : if (!m_cache_state.is_cached(x, common::optimization::ExpressionTerm::JACOBIAN)) {
140 : : evaluate(x, ComputeMode{}.set_jacobian());
141 : : }
142 : : out = m_jacobian;
143 : : }
144 : :
145 : : void hessian_(const DomainValue & x, HessianRef out)
146 : : {
147 : : if (!m_cache_state.is_cached(x, common::optimization::ExpressionTerm::SCORE)) {
148 : : evaluate(x, ComputeMode{}.set_hessian());
149 : : }
150 : : out = m_hessian;
151 : : }
152 : :
153 : : void evaluate(const DomainValue & x, const ComputeMode & mode)
154 : : {
155 : : if (!mode.score() && !mode.jacobian() && !mode.hessian()) {
156 : : return;
157 : : }
158 : : m_cache_state.update(x, mode);
159 : : // Call the implementation method.
160 : : static_cast<Derived *>(this)->evaluate_(x, mode);
161 : : }
162 : :
163 : : protected:
164 : : void set_score(Value score)
165 : : {
166 : : m_score = score;
167 : : }
168 : :
169 : : void set_jacobian(const JacobianRef & jacobian)
170 : : {
171 : : m_jacobian = jacobian;
172 : : }
173 : :
174 : : void set_hessian(const HessianRef & hessian)
175 : : {
176 : : m_hessian = hessian;
177 : : }
178 : :
179 : : private:
180 : : Value m_score;
181 : : Jacobian m_jacobian;
182 : : Hessian m_hessian;
183 : : CacheStateMachine<DomainValue, ComparatorT> m_cache_state;
184 : : };
185 : :
186 : : /// A generalized representation of an optimization problem. Constrained
187 : : /// implementation classes must implement the following methods:
188 : : /// score_opt_(x);
189 : : /// jacobian_opt_(x, out);
190 : : /// hessian_opt_(x, out);
191 : : /// \tparam Derived Implementation class
192 : : /// \tparam DomainValueT The type for parameter to be optimized for.
193 : : /// \tparam NumJacobianColsT Number of columns in the jacobian matrix.
194 : : /// \tparam NumVarsT Number of variables.
195 : : /// \tparam ObjectiveT Objective implementation class.
196 : : /// \tparam EqualityConstraintsT A tuple of equality constraint expressions.
197 : : /// \tparam InequalityConstraintsT A tuple of inequality constraint expressions.
198 : : template<typename Derived, typename DomainValueT, int NumJacobianColsT, int NumVarsT,
199 : : typename ObjectiveT, typename EqualityConstraintsT, typename InequalityConstraintsT>
200 : : class OPTIMIZATION_PUBLIC OptimizationProblem;
201 : :
202 : :
203 : : // Definition of OptimizationProblem.
204 : : // Template specialization is used for type deduction and forwarding constraint parameter packs
205 : : // to the tuples. This way "std::tuple" is enforced to be used on the constrained
206 : : // OptimizationProblem implementations.
207 : : template<typename Derived,
208 : : typename ... EqualityConstraints,
209 : : typename ... InequalityConstraints,
210 : : typename ObjectiveT, typename DomainValueT, int NumJacobianColsT, int NumVarsT>
211 : : class OPTIMIZATION_PUBLIC OptimizationProblem<Derived, DomainValueT, NumJacobianColsT, NumVarsT,
212 : : ObjectiveT,
213 : : std::tuple<EqualityConstraints...>,
214 : : std::tuple<InequalityConstraints...>>
215 : : : public Expression<Derived, DomainValueT, NumJacobianColsT, NumVarsT>
216 : : {
217 : : public:
218 : : using EqualityConstraintsT = std::tuple<EqualityConstraints...>;
219 : : using InequalityConstraintsT = std::tuple<InequalityConstraints...>;
220 : : using Value = autoware::common::types::float64_t;
221 : : using Jacobian = Eigen::Matrix<Value, NumVarsT, NumJacobianColsT>;
222 : : using Hessian = Eigen::Matrix<Value, NumVarsT, NumVarsT>;
223 : : using JacobianRef = Eigen::Ref<Jacobian>;
224 : : using HessianRef = Eigen::Ref<Hessian>;
225 : : static constexpr bool8_t is_unconstrained{(std::tuple_size<EqualityConstraintsT>::value == 0U) &&
226 : : (std::tuple_size<InequalityConstraintsT>::value == 0U)};
227 : :
228 : : OptimizationProblem(
229 : : ObjectiveT && objective,
230 : : EqualityConstraintsT && eq_constraints,
231 : : InequalityConstraintsT && ineq_constraints
232 : : )
233 : : : m_objective(std::forward<ObjectiveT>(objective)),
234 : : m_equality_constraints(std::forward<EqualityConstraintsT>(eq_constraints)),
235 : : m_inequality_constraints(std::forward<InequalityConstraintsT>(ineq_constraints))
236 : : {
237 : : }
238 : :
239 : : template<typename ... Args, typename Dummy = void, typename = std::enable_if_t<
240 : : is_unconstrained, Dummy>>
241 [ + - + - : 3 : OptimizationProblem(
+ - + - +
- + - + -
+ - + - +
- + - + -
+ - + - +
- + - ]
242 : : Args && ... args
243 : : )
244 : : : m_objective(std::forward<Args>(args)...)
245 : : {
246 : : }
247 : :
248 : : template<bool8_t enabled = is_unconstrained>
249 : : typename std::enable_if_t<enabled, Value> score_(const DomainValueT & x)
250 : : {
251 : : return m_objective(x);
252 : : }
253 : :
254 : : template<bool8_t enabled = is_unconstrained>
255 : : typename std::enable_if_t<!enabled, Value> score_(const DomainValueT & x)
256 : : {
257 : : return opt_impl()->score_opt_(x);
258 : : }
259 : :
260 : : template<bool8_t enabled = is_unconstrained>
261 : : typename std::enable_if_t<enabled, void> jacobian_(const DomainValueT & x, JacobianRef out)
262 : : {
263 : 125 : m_objective.jacobian(x, out);
264 : : }
265 : :
266 : : template<bool8_t enabled = is_unconstrained>
267 : : typename std::enable_if_t<!enabled, void> jacobian_(const DomainValueT & x, JacobianRef out)
268 : : {
269 : : opt_impl()->jacobian_opt_(x, out);
270 : : }
271 : :
272 : : template<bool8_t enabled = is_unconstrained>
273 : : typename std::enable_if_t<enabled, void> hessian_(const DomainValueT & x, HessianRef out)
274 : : {
275 : 125 : m_objective.hessian(x, out);
276 : : }
277 : :
278 : : template<bool8_t enabled = is_unconstrained>
279 : : typename std::enable_if_t<!enabled, void> hessian_(const DomainValueT & x, HessianRef out)
280 : : {
281 : : opt_impl()->hessian_opt_(x, out);
282 : : }
283 : :
284 : : template<bool8_t enabled = is_unconstrained>
285 : : typename std::enable_if_t<enabled, void> evaluate(
286 : : const DomainValueT & x,
287 : : const ComputeMode & mode)
288 : : {
289 : : m_objective.evaluate(x, mode);
290 : : }
291 : :
292 : : template<bool8_t enabled = is_unconstrained>
293 : : typename std::enable_if_t<!enabled, void> evaluate(
294 : : const DomainValueT & x,
295 : : const ComputeMode & mode)
296 : : {
297 : : opt_impl()->evaluate_(x, mode);
298 : : }
299 : :
300 : : protected:
301 : : ObjectiveT & objective()
302 : : {
303 : : return m_objective;
304 : : }
305 : :
306 : : EqualityConstraintsT & equality_constraints()
307 : : {
308 : : return m_equality_constraints;
309 : : }
310 : :
311 : : InequalityConstraintsT & inequality_constraints()
312 : : {
313 : : return m_inequality_constraints;
314 : : }
315 : :
316 : : private:
317 : : Derived * opt_impl()
318 : : {
319 : : return static_cast<Derived *>(this);
320 : : }
321 : :
322 : : ObjectiveT m_objective;
323 : : EqualityConstraintsT m_equality_constraints;
324 : : InequalityConstraintsT m_inequality_constraints;
325 : : };
326 : :
327 : : /// Convenience class for unconstrained optimization problems. This is a thin wrapper
328 : : /// around the objective expression.
329 : : /// \tparam ObjectiveT Objective of the optimization problem.
330 : : /// \tparam DomainValueT Parameter type.
331 : : /// \tparam NumVarsT Number of variables.
332 : : template<typename ObjectiveT, typename DomainValueT, int NumVarsT>
333 : : class UnconstrainedOptimizationProblem : public
334 : : OptimizationProblem<UnconstrainedOptimizationProblem<ObjectiveT, DomainValueT, NumVarsT>,
335 : : DomainValueT, 1U, NumVarsT, ObjectiveT, std::tuple<>, std::tuple<>>
336 : : {
337 : : using OptimizationProblem<UnconstrainedOptimizationProblem<ObjectiveT, DomainValueT, NumVarsT>,
338 : : DomainValueT, 1U, NumVarsT, ObjectiveT, std::tuple<>, std::tuple<>>::OptimizationProblem;
339 : : };
340 : :
341 : : } // namespace optimization
342 : : } // namespace common
343 : : } // namespace autoware
344 : :
345 : : #endif // OPTIMIZATION__OPTIMIZATION_PROBLEM_HPP_
|
