18 #include <unordered_set>
45 class OCRoadBasedPrediction:
public OCPredictionStrategy<traffic::Participant>
89 const double ds = 2.0 * std::sqrt(
r*
r-b*b);
92 const double dt = t1-t0;
95 const double k = (int)std::ceil(l/ds);
96 const double s0 = -k*ds*0.5 + 0.5*ds;
97 const double s1 = +k*ds*0.5 + v*dt + 0.5*
a_max_*dt*dt - 0.5*ds;
110 int predictionID =
set.size();
112 center.
getData()(1,0) = pos0(0)-l*0.5*dx;
113 center.
getData()(2,0) = pos0(1)-l*0.5*dy;
114 center.
getData()(3,0) = pos0(2);
116 center.
getData()(1,1) = pos0(0)+l*0.5*dx;
117 center.
getData()(2,1) = pos0(1)+l*0.5*dy;
118 center.
getData()(3,1) = pos0(2);
125 for(
double s = ds*0.5;s<=l-ds*0.5;s+=ds)
127 auto pos = center.
f(s);
135 if(!std::isnan(
r)&&!std::isnan(pos(0))&&!std::isnan(pos(1))&&!std::isnan(t0)&&!std::isnan(t1))
140 set.push_back(prediction);
145 std::unordered_set<adore::env::BorderBased::BorderID,adore::env::BorderBased::BorderIDHasher> closed;
146 std::vector<SearchState> open;
149 itpair.first!=itpair.second;
152 auto it = itpair.first;
153 auto test_participantID = it->first;
154 auto borderID = it->second.first;
155 auto positioning = it->second.second;
156 if(test_participantID==p.
getTrackingID() && positioning.anyInside())
159 x.s0_border_ = positioning.s;
160 x.s0_participant_ = 0.0;
161 x.predecessorID_ = -1;
162 x.borderID_ = borderID;
166 if(bright==
nullptr)
continue;
168 const double lah = (
std::max)(1.0,l*0.5);
169 if(positioning.s>=bright->m_path->limitHi()-0.1)
continue;
170 const auto p0 = bright->m_path->f_bounded(positioning.s-lah);
171 const auto p1 = bright->m_path->f_bounded(positioning.s+lah);
172 const double pdx = p1(0)-
p0(0);
173 const double pdy = p1(1)-
p0(1);
174 if(pdx*pdx+pdy*pdy<0.01)
continue;
175 const double rpdx = pdx*dx + pdy*dy;
176 const double rpdy = -pdx*dy + pdy*dx;
181 int branch_count = 0;
185 int predictionID =
set.size();
188 if(closed.find(
x.borderID_)!=closed.end())
continue;
189 closed.insert(
x.borderID_);
201 const double sx0 =
x.s0_participant_;
202 const double dsmax = center.
limitHi()-
x.s0_border_;
203 const double sx1 =
std::min(s1,sx0+dsmax);
204 const double s_max_border = center.
limitHi();
205 if(center.
getData().nc()>=2)
for(
double s = sx0;s<=sx1;s+=ds)
207 auto pos = center.
f(s+
x.s0_border_-
x.s0_participant_);
208 auto pos_right =
right->f(s+
x.s0_border_-
x.s0_participant_);
209 double r_dynamic = sqrt((pos(0)-pos_right(0))*(pos(0)-pos_right(0)) + (pos(1)-pos_right(1))*(pos(1)-pos_right(1)));
220 if(!std::isnan(r_dynamic)&&!std::isnan(pos(0))&&!std::isnan(pos(1))&&!std::isnan(t0)&&!std::isnan(t1))
225 set.push_back(prediction);
235 auto nextBorder = it.current()->second;
236 if(border->isContinuousPredecessorOf(nextBorder))
240 xnext.s0_participant_ = sx1;
241 xnext.predecessorID_ = predictionID;
242 xnext.borderID_ = nextBorder->m_id;
243 open.push_back(xnext);
252 return branch_count>0;
adore::mad::LLinearPiecewiseFunctionM< double, 4 > getCenterline(const BorderID &id)
get the linear piecewise description of the centerline:
Definition: borderset.h:647
itCoordinate2Border getSuccessors(Border *b)
get an interator pair for all borders which follow after b
Definition: borderset.h:996
Border * getBorder(const BorderID &id) const
retrieve a border by ID
Definition: borderset.h:628
void setAngleErrorMax(double value)
Definition: ocroadbasedprediction_test.h:69
double time_headway_
Definition: ocroadbasedprediction.h:56
void setVMax(double value)
Definition: ocroadbasedprediction_test.h:74
void setAMax(double value)
Definition: ocroadbasedprediction_test.h:75
double lat_error_
Definition: ocroadbasedprediction.h:50
double t_max_utc_
Definition: ocroadbasedprediction.h:48
double v_max_
Definition: ocroadbasedprediction.h:52
void setAMin(double value)
Definition: ocroadbasedprediction_test.h:76
void setTimeHeadway(double value)
Definition: ocroadbasedprediction_test.h:77
OCRoadBasedPrediction(adore::env::traffic::TrafficMap *trafficMap)
Definition: ocroadbasedprediction_test.h:79
double a_min_
Definition: ocroadbasedprediction.h:54
void setLonError(double value)
Definition: ocroadbasedprediction_test.h:73
double lat_precision_
Definition: ocroadbasedprediction.h:49
void setLatError(double value)
Definition: ocroadbasedprediction_test.h:72
double angle_error_max_
Definition: ocroadbasedprediction.h:55
void setTimeLeeway(double value)
Definition: ocroadbasedprediction_test.h:78
double a_max_
Definition: ocroadbasedprediction.h:53
void setTMaxUTC(double value)
Definition: ocroadbasedprediction_test.h:70
double time_leeway_
Definition: ocroadbasedprediction.h:57
double lon_error_
Definition: ocroadbasedprediction.h:51
void setLatPrecision(double value)
Definition: ocroadbasedprediction_test.h:71
adore::env::traffic::TrafficMap * trafficMap_
Definition: ocroadbasedprediction.h:63
virtual bool predict(const traffic::Participant &p, OccupancyCylinderPredictionSet &set) const override
Definition: ocroadbasedprediction_test.h:85
Definition: velocityprediction.h:22
void predict_s_tva(double s0, double v0, double a0, double delay, double a1, double vbound, double t0, double dt, double t1, adore::mad::LLinearPiecewiseFunctionM< double, 3 > &result)
Definition: velocityprediction.h:24
Definition: trafficmap.h:36
adore::env::BorderBased::BorderSet * getBorderSet()
Get the border set.
Definition: trafficmap.h:63
const TParticipantToBorder & getParticipantToBorder() const
Get the participant to border map.
Definition: trafficmap.h:81
virtual CT f(DT x) const override
Definition: llinearpiecewisefunction.h:251
virtual DT limitLo() const override
Definition: llinearpiecewisefunction.h:264
adoreMatrix< T, n+1, 0 > & getData()
Definition: llinearpiecewisefunction.h:147
virtual DT limitHi() const override
Definition: llinearpiecewisefunction.h:259
void insert(const VolumeType &volume)
Definition: vectorbasedvolumetree.h:90
std::vector< OccupancyCylinderPrediction > OccupancyCylinderPredictionSet
Definition: occupancycylinderprediction.h:40
@ right
Definition: indicator_hint.h:36
interval< T > atan2(interval< T > y, interval< T > x)
Definition: intervalarithmetic.h:234
interval< T > cos(interval< T > x)
Definition: intervalarithmetic.h:225
T bound(T lb, T value, T ub)
Definition: adoremath.h:569
T min(T a, T b, T c, T d)
Definition: adoremath.h:663
void set(T *data, T value, int size)
Definition: adoremath.h:39
interval< T > sin(interval< T > x)
Definition: intervalarithmetic.h:204
adoreMatrix< T, N, M > max(adoreMatrix< T, N, M > a, const adoreMatrix< T, N, M > &b)
Definition: adoremath.h:686
x
Definition: adore_set_goal.py:30
p0
Definition: adore_set_pose.py:32
r
Definition: adore_suppress_lanechanges.py:209
Definition: areaofeffectconverter.h:20
This struct identifies a Border by the coordinates of the starting and the end point.
Definition: borderid.h:31
T1 & current()
Definition: borderset.h:50
Definition: ocroadbasedprediction.h:65
double s0_border_
Definition: ocroadbasedprediction.h:67
int predecessorID_
Definition: ocroadbasedprediction.h:70
double s0_participant_
Definition: ocroadbasedprediction.h:68
adore::env::BorderBased::BorderID borderID_
Definition: ocroadbasedprediction.h:69
Definition: occupancycylinderprediction.h:27
int branchID_
Definition: occupancycylinderprediction.h:31
int trackingID_
Definition: occupancycylinderprediction.h:30
adore::mad::OccupancyCylinderTree occupancy_
Definition: occupancycylinderprediction.h:33
float confidence_
Definition: occupancycylinderprediction.h:34
int predecessorID_
Definition: occupancycylinderprediction.h:32
Struct for representing a participant in traffic.
Definition: participant.h:30
double getLength() const
Definition: participant.h:155
TTrackingID getTrackingID() const
Definition: participant.h:109
double getObservationTime() const
Definition: participant.h:112
const adoreMatrix< double, 3, 1 > & getCenter() const
Definition: participant.h:124
double getYaw() const
Definition: participant.h:154
double getWidth() const
Definition: participant.h:156
double getVy() const
Definition: participant.h:158
double getVx() const
Definition: participant.h:157
Definition: occupancycylinder.h:25