22 #include <unordered_map>
53 if(borderSubset->size()==0)
return 0;
54 if(borderSubset->size()==1)
return *borderSubset->begin();
57 double bestValue = 9e99;
58 adoreMatrix<double,3,1> vpos;
62 for(
auto it = borderSubset->begin();it!=borderSubset->end();it++)
76 double sleft =
left->m_path->getPositionOfPoint(vpos(0),vpos(1),1,2,t,n);
77 value += adore::mad::norm2<double,3>(
left->m_path->f(sleft)-vpos);
85 double sright =
right->m_path->getPositionOfPoint(vpos(0),vpos(1),1,2,t,n);
86 value += adore::mad::norm2<double,3>(
right->m_path->f(sright)-vpos);
89 double right_orientation =
std::atan2(
right->m_path->dfidx(sright,1),
right->m_path->dfidx(sright,0));
131 double p0X,p0Y,p1X,p1Y,p2X,p2Y,p3X,p3Y,p4X,p4Y;
136 p0X = ego->
getX() + cpsi * (rho-
m_d);
137 p0Y = ego->
getY() + spsi * (rho-
m_d);
142 p1X = p0X + (rho)*cpsi - (w2)*spsi;
143 p1Y = p0Y + (rho)*spsi + (w2)*cpsi;
145 p2X = p0X + (rho)*cpsi - (-w2)*spsi;
146 p2Y = p0Y + (rho)*spsi + (-w2)*cpsi;
148 p3X = p0X + (-rho)*cpsi - (w2)*spsi;
149 p3Y = p0Y + (-rho)*spsi + (w2)*cpsi;
151 p4X = p0X + (-rho)*cpsi - (-w2)*spsi;
152 p4Y = p0Y + (-rho)*spsi + (-w2)*cpsi;
156 p1X = p0X - spsi*1.1;
157 p1Y = p0Y + cpsi*1.1;
158 p2X = p0X + spsi*1.1;
159 p2Y = p0Y - cpsi*1.1;
166 if(bordersInRegion->size()==0)
174 for(
auto it = bordersInRegion->begin();it!=bordersInRegion->end();it++)
207 decendants.push_back(*it);
213 if( decendants.size()>0 )
258 if(borderSubset->size()==0)
265 if(borderSubset->size()==1)
return *borderSubset->begin();
272 for(
auto it = borderSubset->begin();it!=borderSubset->end();it++)
284 decendants.push_back(*it);
290 bool costSet =
false;
292 if( decendants.size()>0 )
295 for(
auto it = decendants.begin();it!=decendants.end();it++)
309 minCost = nextBorder2Cost->second.getCombinedCost();
312 else if(minCost > nextBorder2Cost->second.getCombinedCost())
315 minCost = nextBorder2Cost->second.getCombinedCost();
Definition: bordercostmap.h:31
Definition: lanematchingstrategy.h:350
double noff
Definition: lanematchingstrategy.h:356
BorderPositioning()
Definition: lanematchingstrategy.h:357
bool m_isPointInside[N]
Definition: lanematchingstrategy.h:352
BorderPositioning(Border *bright, Border *bleft, double Xref, double Yref, double *X, double *Y)
Definition: lanematchingstrategy.h:358
bool allInside()
Definition: lanematchingstrategy.h:373
double s
Definition: lanematchingstrategy.h:354
bool isReferenceInside()
Definition: lanematchingstrategy.h:379
double soff
Definition: lanematchingstrategy.h:355
bool m_isReferenceInside
Definition: lanematchingstrategy.h:353
bool anyInside()
Definition: lanematchingstrategy.h:367
efficiently store borders in boost R-tree
Definition: borderset.h:99
Border * getLeftNeighbor(Border *b)
Get left neighbor of a border.
Definition: borderset.h:1252
bool borderTypeValid(Border *b)
check whether border type is in allowed types of set
Definition: borderset.h:204
Definition: lanematchingstrategy.h:105
bool m_lastMatch_initialized
Definition: lanematchingstrategy.h:107
LMSNearest m_nearestLaneStragey
Definition: lanematchingstrategy.h:109
double m_a
Definition: lanematchingstrategy.h:112
bool m_delayedSwitching
Definition: lanematchingstrategy.h:111
double m_c
Definition: lanematchingstrategy.h:112
double m_d
Definition: lanematchingstrategy.h:112
void setDimensions(double a, double b, double c, double d, double w)
Definition: lanematchingstrategy.h:124
BorderSet * m_borderSet
Definition: lanematchingstrategy.h:110
double m_w
Definition: lanematchingstrategy.h:112
void reset()
Definition: lanematchingstrategy.h:235
virtual Border * getBestMatch(BorderSubSet *bordersInRegion, adore::env::VehicleMotionState9d *ego) override
Definition: lanematchingstrategy.h:128
double m_b
Definition: lanematchingstrategy.h:112
BorderID m_lastMatch
Definition: lanematchingstrategy.h:108
LMSContinuation(BorderSet *borderSet, bool delayedSwitching=false)
Definition: lanematchingstrategy.h:114
Definition: lanematchingstrategy.h:242
LMSContinuation m_continuationStrategy
Definition: lanematchingstrategy.h:248
BorderID m_lastMatch
Definition: lanematchingstrategy.h:246
BorderSet * m_borderSet
Definition: lanematchingstrategy.h:245
BorderCostMap * m_borderID2Cost
Definition: lanematchingstrategy.h:247
bool m_lastMatch_initialized
Definition: lanematchingstrategy.h:244
virtual Border * getBestMatch(BorderSubSet *borderSubset, adore::env::VehicleMotionState9d *ego) override
Definition: lanematchingstrategy.h:256
LMSNavigation(BorderSet *borderSet, BorderCostMap *borderID2Cost)
Definition: lanematchingstrategy.h:250
Definition: lanematchingstrategy.h:38
double m_w_orientation
Definition: lanematchingstrategy.h:41
virtual Border * getBestMatch(BorderSubSet *borderSubset, adore::env::VehicleMotionState9d *ego) override
Definition: lanematchingstrategy.h:47
BorderSet * m_borderSet
Definition: lanematchingstrategy.h:40
LMSNearest(BorderSet *borderSet)
Definition: lanematchingstrategy.h:43
Border * getBestMatch(BorderSubSet *borderSubset, double x, double y, double psi)
Definition: lanematchingstrategy.h:51
Definition: lanematchingstrategy.h:32
virtual Border * getBestMatch(BorderSubSet *borderSubset, adore::env::VehicleMotionState9d *ego)=0
double getPositionOfPoint(T px, T py, int d1, int d2, T &d_tangential_min, T &d_normal_min)
Definition: llinearpiecewisefunction.h:955
bool isPointEnclosed(LLinearPiecewiseFunctionM< T, n > *other, T px, T py, int d1, int d2, bool invert_this=false, bool invert_other=true)
Definition: llinearpiecewisefunction.h:1075
std::vector< Border * > BorderSubSet
Definition: borderset.h:92
@ right
Definition: indicator_hint.h:36
@ left
Definition: indicator_hint.h:35
interval< T > atan2(interval< T > y, interval< T > x)
Definition: intervalarithmetic.h:234
interval< T > cos(interval< T > x)
Definition: intervalarithmetic.h:225
void set(T *data, T value, int size)
Definition: adoremath.h:39
interval< T > sin(interval< T > x)
Definition: intervalarithmetic.h:204
x
Definition: adore_set_goal.py:30
y
Definition: adore_set_goal.py:31
w
Definition: adore_set_pose.py:40
Definition: areaofeffectconverter.h:20
This struct identifies a Border by the coordinates of the starting and the end point.
Definition: borderid.h:31
The border struct contains data of the smallest.
Definition: border.h:62
@ OPPOSITE_DIRECTION
Definition: border.h:507
Tborderpath * m_path
Definition: border.h:70
Direction getNeighborDirection()
Get the direction of the left neighbor.
Definition: border.h:517
BorderID m_id
Definition: border.h:68
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
double getPSI() const
Get the heading.
Definition: vehiclemotionstate9d.h:72