ADORe
ADORe is a modular open source software library and toolkit for decision making, planning, control and simulation of automated vehicles
adaptivesampling.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
17 #include <list>
18 #include <vector>
19 
20 namespace adore
21 {
22  namespace mad
23  {
38  template<typename T, int N>
39  LLinearPiecewiseFunctionM<T, N>* sample_adaptive(ALFunction<T, adoreMatrix<T, N, 1>>* f, ALFunction<T, adoreMatrix<T, N, 1>>* df, T edes, T emax, T xmin, T xmax, T xstep, T precision=(T)0.0001)
40  {
41  //LLinearPiecewiseFunctionM<T,N>* result = new LLinearPiecewiseFunctionM<T,N>();
42  //result->getData().set_size(N+1,(std::max)((T)1,(std::ceil)((f->limitHi()-f->limitLo())/xstep)));
43  std::vector<adoreMatrix<T, N + 1, 1>> buffer;
44  adoreMatrix<T, N + 1, 1> element;
45  T x0, x1, e, dedx;
46  adoreMatrix<T, N, 1> y0, y1, dymin, dymax, ymin, ymax;
47  if(f->limitHi()-f->limitLo()>precision)
48  {
49  x0 =(std::min)(f->limitLo()+precision,f->limitHi());
50  y0 = f->f(x0);
51 
52  //first element
53  element(0, 0) = x0;
54  set_rowm(element, dlib::range(1, N)) = y0;
55  buffer.push_back(element);
56 
57  dedx = edes / xstep;
58  for (;;)
59  {
60  do
61  {
62  xstep = (xstep + edes / dedx) / (T)2;//estimate of required stepsize which hopefully yields the desired error: take mean of current and predicted to prevent overreactions
63  xstep = (std::max)(xmin*(T)0.9999, (std::min)(xstep, xmax));//bound stepsize xmin<xstep<xmax
64  //x1 = (std::min)(x0 + xstep, f->limitHi())-precision); // original
65  x1 = (std::min)(x0 + xstep, f->limitHi()); //mark_ro: trying to fix problem with signals exactly at end of road
66  y1 = f->f(x1);
67  try
68  {
69  df->bound(x0, x1, dymin, dymax);
70  e = (std::max)(norm2<T, N>(y1 - y0 - dymin*(x1 - x0)), norm2<T, N>(y1 - y0 - dymin*(x1 - x0))) / (T)2;// ey <= 0.5 * dx * edydx
71  dedx = e / xstep;//estimate of error for unit step size
72  }catch(...)
73  {
74  e=emax;
75  xstep=xmin;
76  }
77  } while (e > emax && xstep > xmin);
78  x0 = x1;
79  y0 = y1;
80  element(0, 0) = x0;
81  set_rowm(element, dlib::range(1, N)) = y0;
82  buffer.push_back(element);
83  if (x0 >= f->limitHi()-precision*(T)2)break;
84  }
85  }
87  result->getData().set_size(N + 1, buffer.size());
88  int i = 0;
89  for (auto it = buffer.begin(); it != buffer.end(); it++)
90  {
91  set_colm(result->getData(), i++) = *it;
92  }
93  return result;
94  }
95  }
96 }
Definition: alfunction.h:74
Definition: llinearpiecewisefunction.h:139
adoreMatrix< T, n+1, 0 > & getData()
Definition: llinearpiecewisefunction.h:147
LLinearPiecewiseFunctionM< T, N > * sample_adaptive(ALFunction< T, adoreMatrix< T, N, 1 >> *f, ALFunction< T, adoreMatrix< T, N, 1 >> *df, T edes, T emax, T xmin, T xmax, T xstep, T precision=(T) 0.0001)
Definition: adaptivesampling.h:39
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
y0
Definition: adore_set_goal.py:26
y1
Definition: adore_set_pose.py:29
x1
Definition: adore_set_pose.py:28
Definition: areaofeffectconverter.h:20