ADORe
ADORe is a modular open source software library and toolkit for decision making, planning, control and simulation of automated vehicles
centerandlanewidth.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 version for centerline computation
13  * Giovanni Lucente - addition of width computation
14  ********************************************************************************/
15 
16 #pragma once
17 #include <adore/mad/adoremath.h>
18 #include <adore/mad/llinearpiecewisefunction.h>
19 #include <iostream>
20 #include <fstream>
21 
22 namespace adore
23 {
24  namespace mad
25  {
33  inline int computeLaneWidthAndCenter(const adoreMatrix<double,0,0>& left,const adoreMatrix<double,0,0>& right,adoreMatrix<double,0,0>& center)
34  {
35  int N = left.nc();
36  int M = right.nc();
37  // K unused?
38  // int K = N + M - 2;
39  auto rxyz = dlib::range(1,3);
40  auto d = dlib::range(4,4);
41  static const double min_length = 1e-3;
42 
43  //points ordered a-b-c in direction of path
44  adoreMatrix<double,3,1> La,Lb,Lc,Lac;//points on the left
45  adoreMatrix<double,3,1> Ra,Rb,Rc,Rac;//points on the right
46  adoreMatrix<double,3,1> Ca,Cc;//points in the center
47  double D;// distance between the corresponding points
48 
49  //init first points
50  La = dlib::subm(left,rxyz,dlib::range(0,0));
51  Ra = dlib::subm(right,rxyz,dlib::range(0,0));
52  Ca = (La+Ra)*0.5;//average of first points
53  D = adore::mad::norm2<double,3>(La-Ra);//distance of first points
54  Lc = La;
55  Rc = Ra;
56  center(0,0) = 0;
57  dlib::set_subm(center,rxyz,dlib::range(0,0)) = Ca;
58  dlib::set_subm(center,d,dlib::range(0,0)) = D;
59 
60 
61  //indices of c points
62  int i_center = 1;
63  int i_left = 1;
64  int i_right = 1;
65 
66  //lengths
67  double length_Lac=0,length_Rac=0,length_Cac;
68  //progress
69  double progress_RonL,progress_LonR;
70  int counter = 0;
71 
72  while(i_left<N || i_right<M)
73  {
74  //assign c point and progress while a==c
75  while(length_Lac<min_length && i_left<N)
76  {
77  Lc = dlib::subm(left,rxyz,dlib::range(i_left,i_left));
78  Lac = Lc-La;
79  length_Lac = adore::mad::norm2<double,3>(Lac);
80  Lac = Lac / length_Lac;
81  i_left++;
82  }
83  while(length_Rac<min_length && i_right<M)
84  {
85  Rc = dlib::subm(right,rxyz,dlib::range(i_right,i_right));
86  Rac = Rc-Ra;
87  length_Rac = adore::mad::norm2<double,3>(Rac);
88  Rac = Rac / length_Rac;
89  i_right++;
90  }
91  if(length_Lac>min_length)
92  {
93  progress_RonL = dlib::trans(Lac)*(Rc-La) / length_Lac;
94  progress_RonL = (std::max)(0.0,progress_RonL);
95  }
96  else
97  {
98  progress_RonL = 1.0;
99  }
100  if(length_Rac>min_length)
101  {
102  progress_LonR = dlib::trans(Rac)*(Lc-Ra) / length_Rac;
103  progress_LonR = (std::max)(0.0,progress_LonR);
104  }
105  else
106  {
107  progress_LonR = 1.0;
108  }
109 
110  if( std::abs(progress_RonL-progress_LonR)<min_length //same progress -> advance both
111  || (progress_LonR>=1.0 && progress_RonL>=1.0) //both beyond next point -> advance both
112  // || i_left==N && i_right==M
113  )
114  {
115  La = Lc;
116  length_Lac = 0;
117  Ra = Rc;
118  length_Rac = 0;
119  }
120  else
121  {
122  if(progress_RonL<progress_LonR)//less progress on right side --> intermediate point on left side
123  {
124  La = La + Lac * progress_RonL * length_Lac;
125  length_Lac = (1.0-progress_RonL) * length_Lac;
126  Ra = Rc;
127  length_Rac = 0;
128  }
129  else //less progress on left side --> intermediate point on right side
130  {
131  Ra = Ra + Rac * progress_LonR * length_Rac;
132  length_Rac = (1.0-progress_LonR) * length_Rac;
133  La = Lc;
134  length_Lac = 0;
135  }
136  if(i_left==N || i_right==M)
137  {
138  Cc = (La+Ra)*0.5;
139  //@TODO set D to perpendicular distance from La or Ra to Ca->Cc
140  if(i_left<N||i_right<M) D = adore::mad::norm2<double,3>(La-Ra);
141  length_Cac = adore::mad::norm2<double,3>(Cc-Ca);
142  if( length_Cac > min_length )
143  {
144  dlib::set_subm(center,rxyz,dlib::range(i_center,i_center)) = Cc;
145  dlib::set_subm(center,d,dlib::range(i_center,i_center)) = D;
146  center(0,i_center) = center(0,i_center-1) + length_Cac;
147  Ca = Cc;
148  i_center ++;
149  }
150  if(i_left<N)
151  {
152  length_Lac = 0.0;
153  La = Lc;
154  }
155  if(i_right<M)
156  {
157  length_Rac = 0.0;
158  Ra = Rc;
159  }
160  }
161  }
162  Cc = (La+Ra)*0.5;
163  //@TODO set D to perpendicular distance from La or Ra to Ca->Cc
164  if(i_left<N||i_right<M) D = adore::mad::norm2<double,3>(La-Ra);
165  length_Cac = adore::mad::norm2<double,3>(Cc-Ca);
166  if( length_Cac > min_length )
167  {
168  dlib::set_subm(center,rxyz,dlib::range(i_center,i_center)) = Cc;
169  dlib::set_subm(center,d,dlib::range(i_center,i_center)) = D;
170  center(0,i_center) = center(0,i_center-1) + length_Cac;
171  Ca = Cc;
172  i_center ++;
173  }
174  }
175  //possibly missing last point
176  La = dlib::subm(left,rxyz,dlib::range(N-1,N-1));
177  Ra = dlib::subm(right,rxyz,dlib::range(M-1,M-1));
178  Cc = (La+Ra)*0.5;
179  length_Cac = adore::mad::norm2<double,3>(Cc-Ca);
180  if( length_Cac > min_length )
181  {
182  dlib::set_subm(center,rxyz,dlib::range(i_center,i_center)) = Cc;
183  dlib::set_subm(center,d,dlib::range(i_center,i_center)) = D;
184  center(0,i_center) = center(0,i_center-1) + length_Cac;
185  i_center ++;
186  }
187 
188 
189  if(i_center<2)
190  {
191  //centerline computation is degenerate: at least 2 points should be returned
192  La = dlib::subm(left,rxyz,dlib::range(0,0));
193  Lb = dlib::subm(left,rxyz,dlib::range(N-1,N-1));
194  Ra = dlib::subm(right,rxyz,dlib::range(0,0));
195  Rb = dlib::subm(right,rxyz,dlib::range(M-1,M-1));
196  dlib::set_subm(center,rxyz,dlib::range(0,0)) = (La+Ra)*0.5;
197  dlib::set_subm(center,rxyz,dlib::range(1,1)) = (Lb+Rb)*0.5;
198  dlib::set_subm(center,d,dlib::range(0,0)) = adore::mad::norm2<double,3>(La-Ra);
199  dlib::set_subm(center,d,dlib::range(1,1)) = adore::mad::norm2<double,3>(Lb-Rb);
200  i_center = 2;
201  }
202  return i_center;
203  }
204 
205  inline int computeLaneWidthAndCenter(const LLinearPiecewiseFunctionM<double,3>& left,const LLinearPiecewiseFunctionM<double,3>& right,LLinearPiecewiseFunctionM<double,4>& center)
206  {
207  adoreMatrix<double, 0, 0> center_data;
208  center.getData().set_size(5, std::max(left.getData().nc() , right.getData().nc())*2);
209  center_data.set_size(5,std::max(left.getData().nc() , right.getData().nc())*2);
210  int K = adore::mad::computeLaneWidthAndCenter(left.getData(), right.getData(), center_data);
211  center.setData(dlib::colm(center_data, dlib::range(0, K - 1)));
212  return K;
213  }
214  }
215 }
adore::mad::LLinearPiecewiseFunctionM::setData
void setData(const adoreMatrix< T, n+1, 0 > &data)
Definition: llinearpiecewisefunction.h:580
adore::mad::LLinearPiecewiseFunctionM::getData
adoreMatrix< T, n+1, 0 > & getData()
Definition: llinearpiecewisefunction.h:147
adore::env::right
@ right
Definition: indicator_hint.h:36
adore::env::left
@ left
Definition: indicator_hint.h:35
adore::mad::computeLaneWidthAndCenter
int computeLaneWidthAndCenter(const adoreMatrix< double, 0, 0 > &left, const adoreMatrix< double, 0, 0 > &right, adoreMatrix< double, 0, 0 > &center)
Definition: centerandlanewidth.h:33
adore::mad::max
adoreMatrix< T, N, M > max(adoreMatrix< T, N, M > a, const adoreMatrix< T, N, M > &b)
Definition: adoremath.h:686
adore
Definition: areaofeffectconverter.h:20