ADORe
ADORe is a modular open source software library and toolkit for decision making, planning, control and simulation of automated vehicles
lpolynomial.h
Go to the documentation of this file.
1 /********************************************************************************
2  * Copyright (C) 2017-2020 German Aerospace Center (DLR).
3  * Eclipse ADORe, Automated Driving Open Research https://eclipse.org/adore
4  *
5  * This program and the accompanying materials are made available under the
6  * terms of the Eclipse Public License 2.0 which is available at
7  * http://www.eclipse.org/legal/epl-2.0.
8  *
9  * SPDX-License-Identifier: EPL-2.0
10  *
11  * Contributors:
12  * Daniel Heß - initial API and implementation
13  ********************************************************************************/
14 
15 #pragma once
16 
17 #include <adore/mad/alfunction.h>
18 
19 #include <algorithm>
20 
21 namespace adore
22 {
23  namespace mad
24  {
25  template<typename T, int N, int M>
26  void poly_parameter_derivative(adoreMatrix<T, N, M + 1>& p);
27 
28 
33  template<typename T, int N, int M>
34  adoreMatrix<T, N, 1> evaluate_poly(const adoreMatrix<T, N, M + 1>& data, const T & x)
35  {
36  adoreMatrix<T, N, 1> y = dlib::colm(data, M);
37  for (int i = M - 1; i >= 0; i--)
38  {
39  y = y * x;
40  y = y + dlib::colm(data, i);
41  }
42  return y;
43  }
44 
51  template<typename T, int N, int M>
52  adoreMatrix<T, N, M + 1> stretch_poly_parameters(adoreMatrix<T, N, M + 1> data, const T& a, const T& b, const T& c, const T& d)
53  {
54  T s = (b - a) / (d - c);
55  T si = 1;
56  adoreMatrix<T, N, 1> y_tmp;
57  adoreMatrix<T, 1, M + 1> ones_data = dlib::ones_matrix<T>(1, M + 1);
58  adoreMatrix<T, N, M + 1> new_data = dlib::zeros_matrix<T>(N, M + 1);
59  set_colm(new_data, 0) = evaluate_poly<T, N, M>(data, -c*s + a);
60  for (int i = 1; i <= M; i++)
61  {
62  //s^i, has to be accounted for as d/dx y(xs-cs+a) = y^(i)(xs-cs+a)*s^i
63  si *= s;
64  //comparison of first coefficient of derivative at y_s^(i)(0):=y^(i)(-cs+a)
65  poly_parameter_derivative<T, N, M>(data); // p4 * x^4 --> 4 p4 * x^3
66  poly_parameter_derivative<T, 1, M>(ones_data); // 1 * x^4 --> 4 1 * x^3
67  set_colm(new_data, i) = evaluate_poly<T, N, M>(data, -c*s + a) / ones_data(0) * si; // evaluate the derivative i @ -cs+a, devide by factor in front of pi
68  }
69  return new_data;
70  }
75  template<typename T, int N, int M>
76  void poly_parameter_derivative(adoreMatrix<T, N, M + 1>& p)
77  {
78  for (int i = 0; i < M; i++)
79  {
80  dlib::set_colm(p, i) = dlib::colm(p, i + 1) * (T)(i + 1);
81  }
82  dlib::set_colm(p, M) = dlib::zeros_matrix<T>(N, 1);//decrease order
83  }
84 
90  template<typename T, int M>
91  adoreMatrix<T, 1, M + 1> poly_parameter_for_initial_condition(adoreMatrix<T, M + 1, 1> c)
92  {
93  adoreMatrix<T, 1, M + 1> ones_data = dlib::ones_matrix<T>(1, M + 1);
94  adoreMatrix<T, 1, M + 1> new_data = dlib::zeros_matrix<T>(1, M + 1);
95  new_data(0) = c(0);
96  for (int i = 1; i <= M; i++)
97  {
98  poly_parameter_derivative<T, 1, M>(ones_data);
99  new_data(i) = c(i) / ones_data(0);
100  }
101  return new_data;
102  }
103 
108  template<typename T, int N, int M>
109  void bound_poly(const adoreMatrix<T, N, M + 1>& data, const T& x0, const T& x1, adoreMatrix<T, N, 1> & ymin, adoreMatrix<T, N, 1> & ymax)
110  {
111  adoreMatrix<T, N, M + 1> n_data = stretch_poly_parameters<T, N, M>(data, x0, x1, 0, 1);//always stretch to [0,1] interval
112  ymin = colm(n_data, 0);
113  ymax = ymin;
114  adoreMatrix<T, N, 1> b;
115  T k_choose_r; //k �ber r
116  T M_choose_r; //M �ber r
117  for (int k = 0; k <= M; k++)
118  {
119  b = dlib::zeros_matrix<T>(N, 1);
120  for (int r = 0; r <= k; r++)
121  {
122  k_choose_r = (T)adore::mad::binomial(k, r);//k �ber r
123  M_choose_r = (T)adore::mad::binomial(M, r);//M �ber r
124  T c = k_choose_r / M_choose_r;
125  for (int j = 0; j < N; j++)
126  {
127  b(j) += n_data(j, r)*c;
128  }
129  }
130  ymin = adore::mad::min(ymin, b);
131  ymax = adore::mad::max(ymax, b);
132  }
133  }
134 
138  template<typename T, int M>
139  class LPolynomialS :public ALFunction<T, T>
140  {
141  private:
142  typedef T DT;
143  typedef T CT;
144  adoreMatrix<T, 1, M + 1> m_data;
146  public:
147  LPolynomialS(const adoreMatrix<T, 1, M + 1>& data, DT xmin, DT xmax) :m_data(data), m_xmin(xmin), m_xmax(xmax) {}
148  public:
149  virtual CT f(DT x) const override
150  {
151  return evaluate_poly<T, 1, M>(m_data, x)(0);
152  }
153  virtual DT limitHi() const override
154  {
155  return m_xmax;
156  }
157  virtual DT limitLo() const override
158  {
159  return m_xmin;
160  }
161  virtual void setLimits(DT lo, DT hi)override
162  {
163  m_xmin = lo;
164  m_xmax = hi;
165  }
166  virtual ALFunction<DT, CT>* clone()override
167  {
168  return new LPolynomialS(m_data, m_xmin, m_xmax);
169  }
171  {
172  if (M == 0)//already a constant polynomial -> return const zero function
173  {
174  return new LPolynomialS<T, M>(m_data*(T)0, m_xmin, m_xmax);
175  }
176  else
177  {
178  adoreMatrix<T, 1, (M < 0) ? 0 : M> new_data;
179  new_data = dlib::colm(m_data, dlib::range(1, M));
180  for (int i = 0; i < M; i++)
181  {
182  new_data(0, i) = new_data(0, i) * (T)(i + 1);
183  }
184  return new LPolynomialS<T, (M - 1 < 0) ? 0 : M - 1>(new_data, m_xmin, m_xmax);
185  }
186  }
187  virtual void bound(const DT& xmin, const DT& xmax, CT& ymin, CT& ymax)override
188  {
189  adoreMatrix<T, 1, 1> ymin_tmp;
190  adoreMatrix<T, 1, 1> ymax_tmp;
191  bound_poly<T, 1, M>(m_data, xmin, xmax, ymin_tmp, ymax_tmp);
192  ymin = ymin_tmp(0);
193  ymax = ymax_tmp(0);
194  }
195  };
196 
200  template<typename T, int N, int M>
201  class LPolynomialM : public AScalarToN<T, N>
202  {
203  private:
204  adoreMatrix<T, N, M + 1> m_data;//m_data is organized as [p_0,p_1,...,p_M] for a polynomial y=p_0 + x * p_1 + ... + x^M * p_M
207  public:
208  typedef adoreMatrix<T, N, 1> CT;
209  typedef T DT;
210 
211  LPolynomialM(const adoreMatrix<T, N, M + 1>& data, T xmin, T xmax) :m_data(data), m_xmin(xmin), m_xmax(xmax)
212  {
213  for (int i = 0; i < N; i++)
214  {
215  single_dimensions[i] = OneDimension(this, i);
216  }
217  }
218 
219  public:// from ALFunction
220  virtual CT f(DT x) const override
221  {
222  return evaluate_poly<T, N, M>(m_data, x);
223  }
224  virtual DT limitHi() const override
225  {
226  return m_xmax;
227  }
228 
229  virtual DT limitLo() const override
230  {
231  return m_xmin;
232  }
233 
234  virtual void setLimits(DT lo, DT hi) override
235  {
236  m_xmin = lo;
237  m_xmax = hi;
238  }
239  virtual ALFunction<DT, CT>* clone()override
240  {
242  }
244  {
245  if (M <= 0)//already a constant polynomial -> return const zero function
246  {
247  return new LPolynomialM<T, N, M>(m_data*(T)0, m_xmin, m_xmax);
248  }
249  else
250  {
251  adoreMatrix<T, N, M> new_data;
252  new_data = dlib::colm(m_data, dlib::range(1, M));
253  for (int i = 0; i < M; i++)
254  {
255  dlib::set_colm(new_data, i) = dlib::colm(m_data, i + 1) * (T)(i + 1);
256  }
257  return new LPolynomialM<T, N, (M - 1 < 0) ? 0 : M - 1>(new_data, m_xmin, m_xmax);
258  }
259  }
260 
261  virtual void bound(const DT& xmin, const DT& xmax, CT& ymin, CT& ymax) override
262  {
263  bound_poly<T, N, M>(m_data, xmin, xmax, ymin, ymax);
264  }
265 
266  private:
267  class OneDimension :public ALFunction<DT, T>
268  {
269  private:
271  int m_row;
272  public:
274  OneDimension(LPolynomialM* parent, int row) :m_parent(parent), m_row(row) {}
275  virtual T f(DT x) const override { return m_parent->fi(x, m_row); }
276  virtual DT limitHi() const override { return m_parent->limitHi(); }
277  virtual DT limitLo() const override { return m_parent->limitLo(); }
278  virtual void setLimits(DT lo, DT hi)override { throw FunctionNotImplemented(); }
279  virtual ALFunction<DT, T>* clone()override { return new LPolynomialS<T, M>(dlib::rowm(m_parent->m_data, m_row), m_parent->m_xmin, m_parent->m_xmax); }
281  {
282  auto scalar = clone();
283  auto der = scalar->create_derivative();
284  delete scalar;
285  return der;
286  }
287  virtual void bound(const DT& xmin, const DT& xmax, T& ymin, T& ymax) override
288  {
289  adoreMatrix<T, 1, 1> ymin_tmp;
290  adoreMatrix<T, 1, 1> ymax_tmp;
291  bound_poly<T, 1, M>(rowm(m_parent->m_data, m_row), xmin, xmax, ymin_tmp, ymax_tmp);
292  ymin = ymin_tmp(0);
293  ymax = ymax_tmp(0);
294  }
295  };
297  public:// from AScalarToN
299  virtual T fi(T x, int row) const override
300  {
301  return evaluate_poly<T, 1, M>(rowm(m_data, row), x)(0);
302  }
303  virtual SUBFUN* dimension(int i) override
304  {
305  return &single_dimensions[i];
306  }
307  virtual void multiply(adoreMatrix<T, 0, 0> A, int rowi, int rowj)override
308  {
309  dlib::set_subm(m_data, dlib::range(rowi, rowj),dlib::range(1,M)) = A * dlib::subm(m_data, dlib::range(rowi, rowj),dlib::range(1,M));
310  }
311  virtual void add(adoreMatrix<T, 0, 1> b, int rowi, int rowj)override
312  {
313  dlib::set_subm(m_data, dlib::range(rowi, rowj), dlib::range(0, 0)) = subm(m_data, dlib::range(rowi, rowj), dlib::range(0, 0)) + b;
314  }
315  };
316  }
317 }
#define adoreMatrix
Definition: adoremath.h:31
Definition: alfunction.h:74
Definition: alfunction.h:214
T DT
Definition: alfunction.h:216
adoreMatrix< T, N, 1 > CT
Definition: alfunction.h:217
Definition: alfunction.h:38
Definition: lpolynomial.h:268
virtual void setLimits(DT lo, DT hi) override
Definition: lpolynomial.h:278
LPolynomialM * m_parent
Definition: lpolynomial.h:270
virtual ALFunction< DT, T > * clone() override
Definition: lpolynomial.h:279
OneDimension()
Definition: lpolynomial.h:273
OneDimension(LPolynomialM *parent, int row)
Definition: lpolynomial.h:274
int m_row
Definition: lpolynomial.h:271
virtual void bound(const DT &xmin, const DT &xmax, T &ymin, T &ymax) override
Definition: lpolynomial.h:287
virtual T f(DT x) const override
Definition: lpolynomial.h:275
virtual DT limitLo() const override
Definition: lpolynomial.h:277
virtual DT limitHi() const override
Definition: lpolynomial.h:276
virtual ALFunction< DT, T > * create_derivative() override
Definition: lpolynomial.h:280
Definition: lpolynomial.h:202
virtual ALFunction< DT, CT > * clone() override
Definition: lpolynomial.h:239
virtual T fi(T x, int row) const override
Definition: lpolynomial.h:299
virtual void multiply(adoreMatrix< T, 0, 0 > A, int rowi, int rowj) override
Definition: lpolynomial.h:307
virtual ALFunction< DT, CT > * create_derivative() override
Definition: lpolynomial.h:243
LPolynomialM(const adoreMatrix< T, N, M+1 > &data, T xmin, T xmax)
Definition: lpolynomial.h:211
T DT
Definition: lpolynomial.h:209
virtual void setLimits(DT lo, DT hi) override
Definition: lpolynomial.h:234
T m_xmax
Definition: lpolynomial.h:206
virtual SUBFUN * dimension(int i) override
Definition: lpolynomial.h:303
virtual DT limitHi() const override
Definition: lpolynomial.h:224
T m_xmin
Definition: lpolynomial.h:205
virtual DT limitLo() const override
Definition: lpolynomial.h:229
OneDimension single_dimensions[N]
Definition: lpolynomial.h:296
virtual void add(adoreMatrix< T, 0, 1 > b, int rowi, int rowj) override
Definition: lpolynomial.h:311
virtual CT f(DT x) const override
Definition: lpolynomial.h:220
adoreMatrix< T, N, M+1 > m_data
Definition: lpolynomial.h:204
adoreMatrix< T, N, 1 > CT
Definition: lpolynomial.h:208
ALFunction< T, T > SUBFUN
Definition: lpolynomial.h:298
virtual void bound(const DT &xmin, const DT &xmax, CT &ymin, CT &ymax) override
Definition: lpolynomial.h:261
Definition: lpolynomial.h:140
virtual void setLimits(DT lo, DT hi) override
Definition: lpolynomial.h:161
virtual ALFunction< DT, CT > * create_derivative() override
Definition: lpolynomial.h:170
T DT
Definition: lpolynomial.h:142
virtual void bound(const DT &xmin, const DT &xmax, CT &ymin, CT &ymax) override
Definition: lpolynomial.h:187
virtual ALFunction< DT, CT > * clone() override
Definition: lpolynomial.h:166
T CT
Definition: lpolynomial.h:143
DT m_xmin
Definition: lpolynomial.h:145
virtual CT f(DT x) const override
Definition: lpolynomial.h:149
adoreMatrix< T, 1, M+1 > m_data
Definition: lpolynomial.h:144
DT m_xmax
Definition: lpolynomial.h:145
virtual DT limitHi() const override
Definition: lpolynomial.h:153
virtual DT limitLo() const override
Definition: lpolynomial.h:157
LPolynomialS(const adoreMatrix< T, 1, M+1 > &data, DT xmin, DT xmax)
Definition: lpolynomial.h:147
adoreMatrix< T, N, M+1 > stretch_poly_parameters(adoreMatrix< T, N, M+1 > data, const T &a, const T &b, const T &c, const T &d)
Definition: lpolynomial.h:52
adoreMatrix< T, N, 1 > evaluate_poly(const adoreMatrix< T, N, M+1 > &data, const T &x)
Definition: lpolynomial.h:34
void poly_parameter_derivative(adoreMatrix< T, N, M+1 > &p)
Definition: lpolynomial.h:76
adoreMatrix< T, 1, M+1 > poly_parameter_for_initial_condition(adoreMatrix< T, M+1, 1 > c)
Definition: lpolynomial.h:91
void bound_poly(const adoreMatrix< T, N, M+1 > &data, const T &x0, const T &x1, adoreMatrix< T, N, 1 > &ymin, adoreMatrix< T, N, 1 > &ymax)
Definition: lpolynomial.h:109
int binomial(int n, int k)
Definition: adoremath.h:161
T min(T a, T b, T c, T d)
Definition: adoremath.h:663
adoreMatrix< T, N, M > max(adoreMatrix< T, N, M > a, const adoreMatrix< T, N, M > &b)
Definition: adoremath.h:686
x0
Definition: adore_set_goal.py:25
x
Definition: adore_set_goal.py:30
y
Definition: adore_set_goal.py:31
x1
Definition: adore_set_pose.py:28
r
Definition: adore_suppress_lanechanges.py:209
Definition: areaofeffectconverter.h:20