ADORe
ADORe is a modular open source software library and toolkit for decision making, planning, control and simulation of automated vehicles
lanefollowinggeometry.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  * Matthias Nichting
14  ********************************************************************************/
15 
16 
17 #pragma once
18 #include <adore/mad/adoremath.h>
20 #include <adore/mad/centerline.h>
26 
27 #include "csaps.h"
28 
29 namespace adore
30 {
31  namespace env
32  {
33  namespace BorderBased
34  {
35  template <int PolyFitPoints, int PolyEvaluatePoints>
41  {
42 
43  public:
63  double m_s_min;
64  double m_s_max;
65  bool m_view_valid;
74  double m_vehicle_width;
76  double Sbuf[PolyEvaluatePoints];
77  double Xbuf[PolyEvaluatePoints];
78  double Ybuf[PolyEvaluatePoints];
79  double nXbuf[PolyEvaluatePoints];
80  double nYbuf[PolyEvaluatePoints];
81  double dXbuf[PolyEvaluatePoints];
82  double dYbuf[PolyEvaluatePoints];
83  double ddXbuf[PolyEvaluatePoints];
84  double ddYbuf[PolyEvaluatePoints];
85  double dddXbuf[PolyEvaluatePoints];
86  double dddYbuf[PolyEvaluatePoints];
87 
88  public:
94  {
95  m_planning_time = 15.0;
96  m_min_view_distance = 5; // m
97  m_centerSmoothed_fct.setData(dlib::zeros_matrix<double>(4, PolyEvaluatePoints));
98  m_centerSmoothedDerivative1_fct.setData(dlib::zeros_matrix<double>(4, PolyEvaluatePoints));
99  m_centerSmoothedDerivative2_fct.setData(dlib::zeros_matrix<double>(4, PolyEvaluatePoints));
100  m_centerNormal_fct.setData(dlib::zeros_matrix<double>(3, PolyEvaluatePoints));
101  m_centerHeading_fct.setData(dlib::zeros_matrix<double>(2, PolyEvaluatePoints));
102  m_centerSmoothedCurvature_fct.setData(dlib::zeros_matrix<double>(2, PolyEvaluatePoints));
103  m_centerSmoothedCurvatureDerivative_fct.setData(dlib::zeros_matrix<double>(2, PolyEvaluatePoints));
104  m_leftDistance_fct.setData(dlib::zeros_matrix<double>(2, PolyEvaluatePoints));
105  m_rightDistance_fct.setData(dlib::zeros_matrix<double>(2, PolyEvaluatePoints));
106  m_vehicle_width = 1.8;
107  m_view_valid = false;
108  }
123  void update(BorderSet* borderSet,
124  BorderTrace* borderTrace,
125  BorderCostMap* borderCost,
126  Border* start,
128  double lookahead_distance,
129  double lookbehind_distance,
130  double smoothness = 0.05,
131  bool activate_navigation = false
132  )
133  {
134  m_view_valid = false; // set to true at end
135  m_start = start;
136  if(start==nullptr) return; //if the start node is not available, then the view is not valid
137 
138  const double excess_distance_border = 10.0;
139  borderTrace->setDistanceLimit(lookbehind_distance + excess_distance_border);
140 
141  /* go back n steps in view */
142  double s0_border =
143  start->m_path->getClosestParameter(ego->getX(), ego->getY(), 1, 2); // position on current border
144  double walkback_distance = s0_border;
146  m_egoBorderIndex = 0;
147  for (auto it = borderTrace->rbegin(); it != borderTrace->rend() && !(it->first == start->m_id); it++)
148  {
149  auto current = borderSet->getBorder(it->first);
150  if (current == 0)
151  {
152  // the border is out of scope - clear everything to prevent gaps
154  walkback_distance = s0_border;
155  }
156  else
157  {
158  m_rightBorders.append(current, false); //@TODO check how the direction can be determined
159  walkback_distance += it->second;
160  }
161  m_egoBorderIndex++; // the border with the ego vehicle is further to the front
162  }
163 
164  if(activate_navigation)
165  {
166  BorderBased::BASFollowNavigation bas(start,borderSet,borderCost,lookahead_distance+s0_border + excess_distance_border);
167  m_rightBorders.append(&bas);
168  }
169  else
170  {
171  BASFollowStraight bas(start, borderSet, lookahead_distance + s0_border + excess_distance_border);
172  m_rightBorders.append(&bas);
173  }
174 
175  BASNeighbor basNeighbor(m_rightBorders.getBorders(), borderSet,
178  m_leftBorders.append(&basNeighbor);
179 
182 
183  /* create the center line function */
184  adoreMatrix<double, 0, 0> center_data;
185  center_data.set_size(4, m_leftBorder_fct.getData().nc() + m_rightBorder_fct.getData().nc() - 2);
187  m_centerRaw_fct.setData(dlib::colm(center_data, dlib::range(0, K - 1)));
188 
189  /* create s grid for smoothing of centerline */
190  double s0_raw = m_centerRaw_fct.getClosestParameter(ego->getX(), ego->getY(), 1, 2);
191  double review_distance = std::min(walkback_distance, lookbehind_distance);
192  double preview_distance = lookahead_distance;
193  m_s_min = (std::max)(s0_raw - review_distance, (std::min)(s0_raw, m_centerRaw_fct.limitLo() + 0.1));
194  m_s_max = (std::min)(s0_raw + preview_distance, m_centerRaw_fct.limitHi() - 0.1);
196  return; // TODO is this not an error that should be handled?
197 
198  /* sample the raw centerline at grid points */
199  adore::mad::linspace(m_s_min, m_s_max, Sbuf, PolyFitPoints);
200  for (int i = 0; i < PolyFitPoints; i++)
201  {
202  Xbuf[i] = m_centerRaw_fct.fi(Sbuf[i], 0);
203  Ybuf[i] = m_centerRaw_fct.fi(Sbuf[i], 1);
204  }
205 
206 
207  /* smooth centerline - compute piecewise poly regression */
208 
209  csaps::DoubleArray sdata(PolyFitPoints);
210  csaps::DoubleArray xdata(PolyFitPoints);
211  csaps::DoubleArray ydata(PolyFitPoints);
212 
213 
214  // TODO this should be an intermediate solution
215  // it is still way faster than the previous hand rolled
216  // cubic spline implementation, but alternatives
217  // need to be considered for refactoring (e.g. gsl)
218  for (int i = 0; i < PolyFitPoints; ++i)
219  {
220  sdata(i) = Sbuf[i];
221  xdata(i) = Xbuf[i];
222  ydata(i) = Ybuf[i];
223  }
224 
225  csaps::UnivariateCubicSmoothingSpline splineX(sdata, xdata, smoothness);
226  csaps::UnivariateCubicSmoothingSpline splineY(sdata, ydata, smoothness);
227 
228  /* sample smoothed centerline and evaluate its curvature and curv-derivative at grid points*/
229  adore::mad::linspace(m_s_min, m_s_max, Sbuf, PolyEvaluatePoints);
230  csaps::DoubleArray xidata;
231  csaps::DoubleArray dxidata;
232  csaps::DoubleArray ddxidata;
233  csaps::DoubleArray dddxidata;
234  csaps::DoubleArray yidata;
235  csaps::DoubleArray dyidata;
236  csaps::DoubleArray ddyidata;
237  csaps::DoubleArray dddyidata;
238 
239  std::tie(xidata,dxidata,ddxidata,dddxidata) = splineX(PolyEvaluatePoints, sdata);
240  std::tie(yidata,dyidata,ddyidata,dddyidata) = splineY(PolyEvaluatePoints, sdata);
241 
242 
243  for (int i = 0; i < PolyEvaluatePoints; ++i)
244  {
245  Xbuf[i] = xidata(i);
246  dXbuf[i] = dxidata(i);
247  ddXbuf[i] = ddxidata(i);
248  dddXbuf[i] = dddxidata(i);
249  Ybuf[i] = yidata(i);
250  dYbuf[i] = dyidata(i);
251  ddYbuf[i] = ddyidata(i);
252  dddYbuf[i] = dddyidata(i);
253  }
254 
255  double si = Sbuf[0]; // integral: path length
256  double Li = 0.0; // length of path segment
257  double dLi = 1.0; // integral: path length -> derivative ratio
258  for (int i = 0; i < PolyEvaluatePoints; i++)
259  {
260  Sbuf[i] = si;
261  dLi = std::sqrt(dXbuf[i] * dXbuf[i] + dYbuf[i] * dYbuf[i]);
262  if (i + 1 < PolyEvaluatePoints)
263  {
264  double dx = Xbuf[i + 1] - Xbuf[i];
265  double dy = Ybuf[i + 1] - Ybuf[i];
266  Li = (std::sqrt)(dx * dx + dy * dy); // approximate distance along center line
267  }
268 
269  m_centerSmoothed_fct.getData()(0, i) = si;
270  m_centerSmoothed_fct.getData()(1, i) = Xbuf[i];
271  m_centerSmoothed_fct.getData()(2, i) = Ybuf[i];
272 
274  m_centerSmoothedDerivative1_fct.getData()(1, i) = dXbuf[i] / dLi;
275  m_centerSmoothedDerivative1_fct.getData()(2, i) = dYbuf[i] / dLi;
276 
279  ddXbuf[i] / dLi /
280  dLi; //@TODO: approximation: derivative d²q/dt²*(dt/ds)²+dq/dt*d²t/ds² missing part dq/dt*d²t/ds²
282  ddYbuf[i] / dLi /
283  dLi; //@TODO: approximation: derivative d²q/dt²*(dt/ds)²+dq/dt*d²t/ds² missing part dq/dt*d²t/ds²
284 
285  // normal vector
286  nXbuf[i] = -dYbuf[i] / dLi;
287  nYbuf[i] = dXbuf[i] / dLi;
288  m_centerNormal_fct.getData()(0, i) = si;
289  m_centerNormal_fct.getData()(1, i) = nXbuf[i];
290  m_centerNormal_fct.getData()(2, i) = nYbuf[i];
291  m_centerHeading_fct.getData()(0, i) = si;
293 
296  (ddYbuf[i] * dXbuf[i] - ddXbuf[i] * dYbuf[i]) / (dXbuf[i] * dXbuf[i] + dYbuf[i] * dYbuf[i]);
297 
300  (dddYbuf[i] * dXbuf[i] - dddXbuf[i] * dYbuf[i]) / (dXbuf[i] * dXbuf[i] + dYbuf[i] * dYbuf[i]) -
301  (ddYbuf[i] * dXbuf[i] - ddXbuf[i] * dYbuf[i]) * (2.0 * ddYbuf[i] * dYbuf[i] + 2.0 * ddXbuf[i] * dXbuf[i]) /
302  (dXbuf[i] * dXbuf[i] + dYbuf[i] * dYbuf[i]) / (dXbuf[i] * dXbuf[i] + dYbuf[i] * dYbuf[i]);
303 
304  // integrate the path length
305  if (i + 1 < PolyEvaluatePoints)
306  {
307  si += Li;
308  }
309  }
310  m_s_max = si;
311 
312  /* angular correction: correct heading in order to prevent jumps between -pi and pi */
314 
315  /* compute distance from smoothed centerline to borders*/
316  double s_intersect;
317  double distance;
318  double max_intersect_distance = 5.0;//maximum distance from smooth baseline to border
319  bool extend_fringes = false; // true - extend fringes of border in order to guarantee
320  // intersection with smoothed centerline normals
321  m_view_valid = true; // set view valid to false, if lane boundary cannot be found inside max_intersect_distance
322  // left
323  s_intersect = m_leftBorder_fct.limitLo(); // initialize at border's beginning
324  // s_intersect = m_s_min;//initialize at baseline start
325  for (int i = 0; i < PolyEvaluatePoints; i++) // step through smoothed centerline points and compute distance
326  {
327  bool rv = m_leftBorder_fct.getNextIntersectionWithVector2d(s_intersect, Xbuf[i], Ybuf[i], nXbuf[i], nYbuf[i], s_intersect,
328  distance, max_intersect_distance, extend_fringes);
329  // m_view_valid = m_view_valid && rv;
330  m_leftDistance_fct.getData()(0, i) = Sbuf[i];
331  m_leftDistance_fct.getData()(1, i) = distance;
332  }
333  // right
334  s_intersect = m_rightBorder_fct.limitLo(); // initialize at border's beginning
335  // s_intersect = m_s_min;//initialize at baseline start
336  for (int i = 0; i < PolyEvaluatePoints; i++) // step through smoothed centerline points and compute distance
337  {
338  bool rv = m_rightBorder_fct.getNextIntersectionWithVector2d(s_intersect, Xbuf[i], Ybuf[i], -nXbuf[i], -nYbuf[i],
339  s_intersect, distance, max_intersect_distance, extend_fringes);
340  // m_view_valid = m_view_valid && rv;
341  m_rightDistance_fct.getData()(0, i) = Sbuf[i];
342  m_rightDistance_fct.getData()(1, i) = -distance;
343  }
344  bool width_open = false;
345  m_s_lane_width_open = 0.0;
346  m_s_lane_width_closed = 10000.0;
348  for (int i = 0; i < left.nc(); i++)
349  {
350  double d = (std::abs)(left(1, i)) + (std::abs)(m_rightDistance_fct.fi(left(0, i), 0)) - m_vehicle_width;
351  if (width_open)
352  {
353  if (d < 0)
354  {
355  m_s_lane_width_closed = left(0, i);
356  break;
357  }
358  }
359  else
360  {
361  if (d > 0)
362  {
363  m_s_lane_width_open = left(0, i);
364  width_open = true;
365  }
366  }
367  }
368 
369  m_view_valid = computeNavigationCost(activate_navigation,borderCost);
370 
371  }
372 
373 
374  bool computeNavigationCost(bool activate_navigation,BorderCostMap* borderCostMap)
375  {
376  if(activate_navigation)
377  {
378  double d_sample = 5.0;
379  std::vector<adoreMatrix<double,3,1>> navcost_vector;
380  for(Border* rb:(*m_rightBorders.getBorders()))
381  {
382  auto c = borderCostMap->find(rb->m_id);
383  if(c==borderCostMap->end())return false;
384  adoreMatrix<double,3,1> value;
385  value(0) = c->second.getDistanceToGoal();
386  value(1) = rb->m_id.m_first.m_X;
387  value(2) = rb->m_id.m_first.m_Y;
388  navcost_vector.push_back(value);
389  if(rb->m_path!=nullptr && rb->getLength()>d_sample)
390  {
391  for(double d = d_sample;d<rb->getLength();d+=d_sample)
392  {
393  auto p = rb->m_path->f(d);
394  value(0) = c->second.getDistanceToGoal() + d;
395  value(1) = p(0);
396  value(2) = p(1);
397  navcost_vector.push_back(value);
398  }
399  }
400  }
401  {
402  Border* rb = *m_rightBorders.getBorders()->rbegin();
403  auto c = borderCostMap->find(rb->m_id);
404  if(c==borderCostMap->end())return false;
405  adoreMatrix<double,3,1> value;
406  value(0) = std::max(0.0,c->second.getDistanceToGoal()-rb->getLength());
407  value(1) = rb->m_id.m_last.m_X;
408  value(2) = rb->m_id.m_last.m_Y;
409  navcost_vector.push_back(value);
410  if(rb->m_path!=nullptr && rb->getLength()>d_sample)
411  {
412  for(double d = d_sample;d<rb->getLength();d+=d_sample)
413  {
414  auto p = rb->m_path->f(d);
415  value(0) = c->second.getDistanceToGoal() + d;
416  value(1) = p(0);
417  value(2) = p(1);
418  navcost_vector.push_back(value);
419  }
420  }
421  }
422  //project navigation cost points to baseline
423  std::vector<double> svalues;
424  std::vector<double> cvalues;
427  for(auto& value:navcost_vector)
428  {
429  double tmp=0;//not required
430  double s = m_centerSmoothed_fct.getClosestParameter(value(1),value(2),1,2,tmp);
431  double c;//cost
432  if(s==m_centerSmoothed_fct.limitLo())//potentially located before interval start
433  {
434  double dx = value(1)-xstart(0);
435  double dy = value(2)-xstart(1);
436  double d = std::sqrt(dx*dx+dy*dy);
437  if(d>1.5*d_sample)continue;
438  c = std::max(0.0,value(0) - d);
439  }
440  else if(s==m_centerSmoothed_fct.limitHi())//potentially located after interval end
441  {
442  double dx = value(1)-xend(0);
443  double dy = value(2)-xend(1);
444  double d = std::sqrt(dx*dx+dy*dy);
445  if(d>1.5*d_sample)continue;
446  c = std::max(0.0,value(0) + d);
447  }
448  else
449  {
450  c = value(0);
451  }
452  if( svalues.size()>0 && s<svalues[svalues.size()-1])continue;
453  else if( svalues.size()>0 && s==svalues[svalues.size()-1])
454  {
455  if(c<cvalues[cvalues.size()-1])
456  {
457  svalues[svalues.size()-1] = s;
458  cvalues[svalues.size()-1] = c;
459  }
460  }
461  else
462  {
463  svalues.push_back(s);
464  cvalues.push_back(c);
465  }
466  }
467  if(svalues.size()>0)
468  {
469  m_navigationCost_fct.setData(dlib::zeros_matrix<double>(2, svalues.size()));
470  for(int i=0;i<svalues.size();i++)
471  {
472  m_navigationCost_fct.getData()(0,i) = svalues[i];
473  m_navigationCost_fct.getData()(1,i) = cvalues[i];
474  }
475  }
476  else
477  {
478  m_navigationCost_fct.setData(dlib::zeros_matrix<double>(2, 2));
481  m_navigationCost_fct.getData()(1,0) = 1e10;
482  m_navigationCost_fct.getData()(1,1) = 1e10;
483  }
484  }
485  else
486  {
487  m_navigationCost_fct.setData(dlib::zeros_matrix<double>(2, 2));
490  m_navigationCost_fct.getData()(1,0) = 1.0e10;
491  m_navigationCost_fct.getData()(1,1) = 1.0e10;
492  }
493  return true;
494  }
495 
509  void excludeObstaclePoint(double X, double Y, double s_min, double s_safety, double n_safety, double s_off = 0.0)
510  {
511  double s, n;
512  this->toLocalCoordinates(X, Y, s, n);
514  ;
515  if (s > s_min)
516  {
517  double l = m_leftDistance_fct(s);
518  double r = m_rightDistance_fct(s);
519  if (n > 0.0)
520  {
521  for (int i = m_leftDistance_fct.findIndex((std::max)(s - s_safety, m_centerSmoothed_fct.limitLo()));
522  i < m_leftDistance_fct.getData().nc(); i++)
523  {
524  m_leftDistance_fct.getData()(1, i) = (std::min)(n - n_safety, m_leftDistance_fct.getData()(1, i));
525  if (m_leftDistance_fct.getData()(0, i) > s + s_safety)
526  break;
527  }
528  }
529  else
530  {
531  for (int i = m_rightDistance_fct.findIndex((std::max)(s - s_safety, m_centerSmoothed_fct.limitLo()));
532  i < m_rightDistance_fct.getData().nc(); i++)
533  {
534  m_rightDistance_fct.getData()(1, i) = (std::max)(n + n_safety, m_rightDistance_fct.getData()(1, i));
535  if (m_rightDistance_fct.getData()(0, i) > s + s_safety)
536  break;
537  }
538  }
539  }
540  }
547  {
548  return &m_rightBorders;
549  }
556  {
557  return &m_leftBorders;
558  }
564  Border* getBestMatchingBorder(double X,double Y,double max_deviation)
565  {
566  double s,n;
567  toRelativeCoordinates(X,Y,s,n);
568  if( s<=0.0 || s>=getViewingDistance() )return nullptr;
569  double nmin = - max_deviation + (std::min)(getOffsetOfLeftBorder(s),getOffsetOfRightBorder(s));
570  double nmax = + max_deviation + (std::max)(getOffsetOfLeftBorder(s),getOffsetOfRightBorder(s));
571  if(n<nmin||nmax<n)
572  {
573  return nullptr;
574  }
575 
576  double Xc,Yc,Zc;
577  toEucledianCoordinates(s,0,Xc,Yc,Zc);
578 
579  auto it_left = m_leftBorders.getBorders()->begin();
580  auto it_right = m_rightBorders.getBorders()->begin();
581 
582  while (it_left!=m_leftBorders.getBorders()->end()
583  && it_right!=m_rightBorders.getBorders()->end())
584  {
585  if((*it_right)->isPointInsideLane(*it_left,Xc,Yc))
586  {
587  return *it_right;
588  }
589  else
590  {
591  it_left ++;
592  it_right ++;
593  }
594 
595  }
596  return nullptr;
597  }
598 
599 
606  bool isValid() const
607  {
608  return m_view_valid;
609  }
615  double getViewingDistance() const
616  {
617  return m_s_max - m_s_min;
618  }
619  double getSMax()const
620  {
621  return m_s_max;
622  }
623  double getSMin()const
624  {
625  return m_s_min;
626  }
632  double getProgressOfWidthOpen() const
633  {
634  return m_s_lane_width_open;
635  }
642  {
643  return m_s_lane_width_closed;
644  }
651  {
652  // return &m_centerRaw_fct;
653  return &m_centerSmoothed_fct;
654  }
661  {
662  return &m_centerNormal_fct;
663  }
671  double getCurvature(double s, int derivative)
672  {
674  if (derivative<=0)
675  {
677  }
678  else if(derivative<=1)
679  {
681  }
682  else
683  {
684  return 0.0;
685  }
686  }
692  BAContainer::iterator begin()
693  {
694  return m_rightBorders.getBorders()->begin();
695  }
701  BAContainer::iterator end()
702  {
703  return m_rightBorders.getBorders()->end();
704  }
711  double getHeading(double s)
712  {
714  return m_centerHeading_fct(s);
715  }
722  double getOffsetOfLeftBorder(double s)
723  {
725  return m_leftDistance_fct(s);
726  }
733  {
734  return &m_leftDistance_fct;
735  }
742  double getOffsetOfRightBorder(double s)
743  {
745  return m_rightDistance_fct(s);
746  }
753  {
754  return &m_rightDistance_fct;
755  }
756 
758  {return &m_leftBorder_fct;}
760  {return &m_rightBorder_fct;}
761 
770  void toRelativeCoordinates(double Xg, double Yg, double& s, double& n)
771  {
773  {
774  s = m_centerSmoothed_fct.getClosestParameter(Xg, Yg, 1, 2, n);
775  }
776  s-=m_s_min;
777  }
787  void toEucledianCoordinates(double s, double n, double& Xg, double& Yg, double& Zg)
788  {
791  Zg=0.0;
792  }
793  };
794  } // namespace BorderBased
795  } // namespace env
796 } // namespace adore
This class chooses the successor with the lowest cost until an upper limit on distance is reached.
Definition: borderaccumulator.h:136
This class chooses the straightest successor of a border until an upper limit on distance is reached.
Definition: borderaccumulator.h:78
This class choses the left/right neighbors of a border sequence.
Definition: borderaccumulator.h:231
@ LEFT
Definition: borderaccumulator.h:242
This class collects a sequence of borders, according to chosen BorderAccumulationStrategy.
Definition: borderaccumulator.h:293
void append(Border *current, bool inverted)
‍**
Definition: borderaccumulator.h:353
void clear()
Clear the BorderAccumulator.
Definition: borderaccumulator.h:312
BAContainer * getBorders()
Get the Accumulated Borders.
Definition: borderaccumulator.h:386
void defineFunction(function_type &f)
Create (with new) a function, which contains all accumulated border paths.
Definition: borderaccumulator.h:395
Definition: bordercostmap.h:31
efficiently store borders in boost R-tree
Definition: borderset.h:99
Border * getBorder(const BorderID &id) const
retrieve a border by ID
Definition: borderset.h:628
Definition: bordertrace.h:30
CTYPE::reverse_iterator rend()
Definition: bordertrace.h:70
CTYPE::reverse_iterator rbegin()
Definition: bordertrace.h:69
void setDistanceLimit(double distance_limit)
Definition: bordertrace.h:41
A class with a geometry description of the current lane.
Definition: lanefollowinggeometry.h:41
double m_s_lane_width_open
Definition: lanefollowinggeometry.h:72
void toRelativeCoordinates(double Xg, double Yg, double &s, double &n)
Transform from euclidian to relative coordinates.
Definition: lanefollowinggeometry.h:770
function_type_scalar m_navigationCost_fct
Definition: lanefollowinggeometry.h:61
double getOffsetOfLeftBorder(double s)
Get the offset of the left border at a certain position.
Definition: lanefollowinggeometry.h:722
double getViewingDistance() const
Get the viewing distance.
Definition: lanefollowinggeometry.h:615
double getProgressOfWidthOpen() const
Get the s-coordinate where the lane reaches the required width.
Definition: lanefollowinggeometry.h:632
function_type_xyz m_leftBorder_fct
Definition: lanefollowinggeometry.h:49
void update(BorderSet *borderSet, BorderTrace *borderTrace, BorderCostMap *borderCost, Border *start, adore::env::VehicleMotionState9d *ego, double lookahead_distance, double lookbehind_distance, double smoothness=0.05, bool activate_navigation=false)
update the road geometry
Definition: lanefollowinggeometry.h:123
double dXbuf[PolyEvaluatePoints]
Definition: lanefollowinggeometry.h:81
double getSMin() const
Definition: lanefollowinggeometry.h:623
double getHeading(double s)
Get the heading at a certain position.
Definition: lanefollowinggeometry.h:711
BorderAccumulator * getLeftBorders()
Get the left borders of the LaneFollowingGeometry.
Definition: lanefollowinggeometry.h:555
double Xbuf[PolyEvaluatePoints]
Definition: lanefollowinggeometry.h:77
double getProgressOfWidthClosed() const
Get the s-coordinate where the lane stops to have the required width.
Definition: lanefollowinggeometry.h:641
BorderAccumulator m_rightBorders
Definition: lanefollowinggeometry.h:67
int m_egoBorderIndex
Definition: lanefollowinggeometry.h:71
Border * m_start
Definition: lanefollowinggeometry.h:70
function_type2d m_centerNormal_fct
Definition: lanefollowinggeometry.h:55
function_type_xyz m_centerRaw_fct
Definition: lanefollowinggeometry.h:51
double m_s_lane_width_closed
Definition: lanefollowinggeometry.h:73
bool m_view_valid
Definition: lanefollowinggeometry.h:65
double nXbuf[PolyEvaluatePoints]
Definition: lanefollowinggeometry.h:79
BAContainer::iterator begin()
Get the begin()-iterator of the BAContainer of the right borders.
Definition: lanefollowinggeometry.h:692
BorderAccumulator * getRightBorders()
Get the right borders of the LaneFollowingGeometry.
Definition: lanefollowinggeometry.h:546
velocity_profile m_velocity_fct
Definition: lanefollowinggeometry.h:48
function_type_xyz * getLeftBorderFct()
Definition: lanefollowinggeometry.h:757
BorderAccumulator m_leftBorders
Definition: lanefollowinggeometry.h:68
function_type_scalar m_centerHeading_fct
Definition: lanefollowinggeometry.h:56
adore::mad::function_type_scalar function_type_scalar
Definition: lanefollowinggeometry.h:46
function_type_scalar m_centerSmoothedCurvature_fct
Definition: lanefollowinggeometry.h:57
double m_planning_time
Definition: lanefollowinggeometry.h:62
double Sbuf[PolyEvaluatePoints]
Definition: lanefollowinggeometry.h:76
double dddYbuf[PolyEvaluatePoints]
Definition: lanefollowinggeometry.h:86
int m_startIndexInRightBorders
Definition: lanefollowinggeometry.h:69
double ddYbuf[PolyEvaluatePoints]
Definition: lanefollowinggeometry.h:84
double m_s_max
Definition: lanefollowinggeometry.h:64
double m_min_view_distance
Definition: lanefollowinggeometry.h:66
function_type_xyz m_centerSmoothedDerivative1_fct
Definition: lanefollowinggeometry.h:53
function_type_scalar m_leftDistance_fct
Definition: lanefollowinggeometry.h:59
BAContainer::iterator end()
Get the end()-iterator of the BAContainer of the right borders.
Definition: lanefollowinggeometry.h:701
Border * getBestMatchingBorder(double X, double Y, double max_deviation)
Get the best matching border for a given ego position.
Definition: lanefollowinggeometry.h:564
double getOffsetOfRightBorder(double s)
Get the offset of the right border at a certain position.
Definition: lanefollowinggeometry.h:742
void toEucledianCoordinates(double s, double n, double &Xg, double &Yg, double &Zg)
Transform from relative to euclidian coordinates.
Definition: lanefollowinggeometry.h:787
double Ybuf[PolyEvaluatePoints]
Definition: lanefollowinggeometry.h:78
double m_vehicle_width
Definition: lanefollowinggeometry.h:74
function_type_xyz m_centerSmoothedDerivative2_fct
Definition: lanefollowinggeometry.h:54
double m_s_min
Definition: lanefollowinggeometry.h:63
double ddXbuf[PolyEvaluatePoints]
Definition: lanefollowinggeometry.h:83
bool computeNavigationCost(bool activate_navigation, BorderCostMap *borderCostMap)
Definition: lanefollowinggeometry.h:374
bool isValid() const
Check whether the LaneFollowingGeometry is valid.
Definition: lanefollowinggeometry.h:606
function_type_xyz * getRightBorderFct()
Definition: lanefollowinggeometry.h:759
double nYbuf[PolyEvaluatePoints]
Definition: lanefollowinggeometry.h:80
double dYbuf[PolyEvaluatePoints]
Definition: lanefollowinggeometry.h:82
function_type_scalar * getOffsetOfLeftBorderFct()
Get the function that holds the offset of left borders.
Definition: lanefollowinggeometry.h:732
function_type2d * getCenterlineNormal()
Get the Centerline Normal object.
Definition: lanefollowinggeometry.h:660
function_type_xyz * getCenterline()
Get the centerline of the lane.
Definition: lanefollowinggeometry.h:650
function_type_xyz m_centerSmoothed_fct
Definition: lanefollowinggeometry.h:52
function_type_scalar m_centerSmoothedCurvatureDerivative_fct
Definition: lanefollowinggeometry.h:58
function_type_xyz m_rightBorder_fct
Definition: lanefollowinggeometry.h:50
void excludeObstaclePoint(double X, double Y, double s_min, double s_safety, double n_safety, double s_off=0.0)
Modify a lane boundary to exclude a given point.
Definition: lanefollowinggeometry.h:509
double getCurvature(double s, int derivative)
Get the curvature of the lane at a certain position.
Definition: lanefollowinggeometry.h:671
LaneFollowingGeometry()
Construct a new LaneFollowingGeometry object.
Definition: lanefollowinggeometry.h:93
double dddXbuf[PolyEvaluatePoints]
Definition: lanefollowinggeometry.h:85
function_type_scalar * getOffsetOfRightBorderFct()
Get the function that holds the offset of right borders.
Definition: lanefollowinggeometry.h:752
double getSMax() const
Definition: lanefollowinggeometry.h:619
function_type_scalar m_rightDistance_fct
Definition: lanefollowinggeometry.h:60
virtual CT f(DT x) const override
Definition: llinearpiecewisefunction.h:251
void setData(const adoreMatrix< T, n+1, 0 > &data)
Definition: llinearpiecewisefunction.h:580
double getClosestParameter(T px, T py, int d1, int d2, T &n_min) const
Definition: llinearpiecewisefunction.h:1014
virtual T fi(DT x, int row) const override
Definition: llinearpiecewisefunction.h:391
virtual DT limitLo() const override
Definition: llinearpiecewisefunction.h:264
unsigned int findIndex(DT x, DT precision=0.001) const
Definition: llinearpiecewisefunction.h:158
adoreMatrix< T, n+1, 0 > & getData()
Definition: llinearpiecewisefunction.h:147
virtual DT limitHi() const override
Definition: llinearpiecewisefunction.h:259
bool getNextIntersectionWithVector2d(T x0, T px, T py, T vx, T vy, T &x_result, T &distance, T max_distance, bool extend_fringes=false)
Definition: llinearpiecewisefunction.h:850
@ left
Definition: indicator_hint.h:35
interval< T > atan2(interval< T > y, interval< T > x)
Definition: intervalarithmetic.h:234
void createAngularContinuity(adoreMatrix< T, N, 0 > &data, int row)
Definition: adoremath.h:200
adore::mad::LLinearPiecewiseFunctionM< double, 3 > function_type_xyz
Definition: linearfunctiontypedefs.h:22
adore::mad::LLinearPiecewiseFunctionM< double, 1 > function_type_scalar
Definition: linearfunctiontypedefs.h:24
adoreMatrix< T, 1, n > linspace(T x0, T x1)
Definition: adoremath.h:91
T bound(T lb, T value, T ub)
Definition: adoremath.h:569
adore::mad::LLinearPiecewiseFunctionM< double, 2 > function_type2d
Definition: linearfunctiontypedefs.h:23
bool toRelativeWithNormalExtrapolation(double qX, double qY, const T1 pi, const T1 pj, const T2 ni, const T2 nj, double &s, double &t)
Transformation from Euclidean coordinate system to a relative coordinate system represented by linear...
Definition: adoremath.h:321
T min(T a, T b, T c, T d)
Definition: adoremath.h:663
int computeCenterline(const adoreMatrix< double, 0, 0 > &left, const adoreMatrix< double, 0, 0 > &right, adoreMatrix< double, 0, 0 > &center)
Definition: centerline.h:29
adoreMatrix< T, N, M > max(adoreMatrix< T, N, M > a, const adoreMatrix< T, N, M > &b)
Definition: adoremath.h:686
bool fromRelative(double s, double t, const T1 pi, const T1 pj, const T2 ni, const T2 nj, double &X, double &Y, double &Z)
Transform from relative coordinates to Euclidean coordinates.
Definition: adoremath.h:475
r
Definition: adore_suppress_lanechanges.py:209
Definition: areaofeffectconverter.h:20
Coordinate m_last
Definition: borderid.h:32
The border struct contains data of the smallest.
Definition: border.h:62
Tborderpath * m_path
Definition: border.h:70
double getLength()
Get the length of the border.
Definition: border.h:703
BorderID m_id
Definition: border.h:68
double m_Y
Definition: coordinate.h:35
double m_X
Definition: coordinate.h:35
This struct holds the motion state of the vehicle in 9d.
Definition: vehiclemotionstate9d.h:39
double getX() const
Get the x-coordinate.
Definition: vehiclemotionstate9d.h:54
double getY() const
Get the y-coordinate.
Definition: vehiclemotionstate9d.h:60