ADORe
ADORe is a modular open source software library and toolkit for decision making, planning, control and simulation of automated vehicles
centerline.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 #include <adore/mad/adoremath.h>
18 
19 namespace adore
20 {
21  namespace mad
22  {
29  inline int computeCenterline(const adoreMatrix<double,0,0>& left,const adoreMatrix<double,0,0>& right,adoreMatrix<double,0,0>& center)
30  {
31  int N = left.nc();
32  int M = right.nc();
33  // K unused?
34  // int K = N + M - 2;
35  auto rxyz = dlib::range(1,3);
36  static const double min_length = 1e-3;
37 
38  //points ordered a-b-c in direction of path
39  adoreMatrix<double,3,1> La,Lb,Lc,Lac;//points on the left
40  adoreMatrix<double,3,1> Ra,Rb,Rc,Rac;//points on the right
41  adoreMatrix<double,3,1> Ca,Cc;//points in the center
42 
43  //init first points
44  La = dlib::subm(left,rxyz,dlib::range(0,0));
45  Ra = dlib::subm(right,rxyz,dlib::range(0,0));
46  Ca = (La+Ra)*0.5;//average of first points
47  Lc = La;
48  Rc = Ra;
49  center(0,0) = 0;
50  dlib::set_subm(center,rxyz,dlib::range(0,0)) = Ca;
51 
52  //indices of c points
53  int i_center = 1;
54  int i_left = 1;
55  int i_right = 1;
56 
57  //lengths
58  double length_Lac=0,length_Rac=0,length_Cac;
59  //progress
60  double progress_RonL,progress_LonR;
61 
62  while(i_left<N || i_right<M)
63  {
64  //assign c point and progress while a==c
65  while(length_Lac<min_length && i_left<N)
66  {
67  Lc = dlib::subm(left,rxyz,dlib::range(i_left,i_left));
68  Lac = Lc-La;
69  length_Lac = adore::mad::norm2<double,3>(Lac);
70  Lac = Lac / length_Lac;
71  i_left++;
72  }
73  while(length_Rac<min_length && i_right<M)
74  {
75  Rc = dlib::subm(right,rxyz,dlib::range(i_right,i_right));
76  Rac = Rc-Ra;
77  length_Rac = adore::mad::norm2<double,3>(Rac);
78  Rac = Rac / length_Rac;
79  i_right++;
80  }
81  if(length_Lac>min_length)
82  {
83  progress_RonL = dlib::trans(Lac)*(Rc-La) / length_Lac;
84  progress_RonL = (std::max)(0.0,progress_RonL);
85  }
86  else
87  {
88  progress_RonL = 1.0;
89  }
90  if(length_Rac>min_length)
91  {
92  progress_LonR = dlib::trans(Rac)*(Lc-Ra) / length_Rac;
93  progress_LonR = (std::max)(0.0,progress_LonR);
94  }
95  else
96  {
97  progress_LonR = 1.0;
98  }
99  if( std::abs(progress_RonL-progress_LonR)<min_length
100  || (progress_LonR>=1.0 && progress_RonL>=1.0)
101  || i_left==N || i_right==M)//same progress -> advance both
102  {
103  La = Lc;
104  length_Lac = 0;
105  Ra = Rc;
106  length_Rac = 0;
107  }
108  else
109  {
110  if(progress_RonL<progress_LonR)//less progress on right side --> intermediate point on left side
111  {
112  La = La + Lac * progress_RonL * length_Lac;
113  length_Lac = (1.0-progress_RonL) * length_Lac;
114  Ra = Rc;
115  length_Rac = 0;
116  }
117  else //less progress on left side --> intermediate point on right side
118  {
119  Ra = Ra + Rac * progress_LonR * length_Rac;
120  length_Rac = (1.0-progress_LonR) * length_Rac;
121  La = Lc;
122  length_Lac = 0;
123  }
124  }
125  Cc = (La+Ra)*0.5;
126  length_Cac = adore::mad::norm2<double,3>(Cc-Ca);
127  if( length_Cac > min_length )
128  {
129  dlib::set_subm(center,rxyz,dlib::range(i_center,i_center)) = Cc;
130  center(0,i_center) = center(0,i_center-1) + length_Cac;
131  Ca = Cc;
132  i_center ++;
133  }
134  }
135  if(i_center<2)
136  {
137  //centerline computation is degenerate: at least 2 points should be returned
138  La = dlib::subm(left,rxyz,dlib::range(0,0));
139  Lb = dlib::subm(left,rxyz,dlib::range(N-1,N-1));
140  Ra = dlib::subm(right,rxyz,dlib::range(0,0));
141  Rb = dlib::subm(right,rxyz,dlib::range(M-1,M-1));
142  dlib::set_subm(center,rxyz,dlib::range(0,0)) = (La+Ra)*0.5;
143  dlib::set_subm(center,rxyz,dlib::range(1,1)) = (Lb+Rb)*0.5;
144  i_center = 2;
145  }
146  return i_center;
147  }
148 
150  {
151  adoreMatrix<double, 0, 0> center_data;
152  center.getData().set_size(4, left.getData().nc() + right.getData().nc() - 2);
153  center_data.set_size(4, left.getData().nc() + right.getData().nc() - 2);
154  int K = adore::mad::computeCenterline(left.getData(), right.getData(), center_data);
155  center.setData(dlib::colm(center_data, dlib::range(0, K - 1)));
156  return K;
157  }
158  }
159 }
void setData(const adoreMatrix< T, n+1, 0 > &data)
Definition: llinearpiecewisefunction.h:580
adoreMatrix< T, n+1, 0 > & getData()
Definition: llinearpiecewisefunction.h:147
@ right
Definition: indicator_hint.h:36
@ left
Definition: indicator_hint.h:35
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
Definition: areaofeffectconverter.h:20