Fix lane width (#2520)
* use prob modifiers in lane width calc * update ref * update ref
This commit is contained in:
parent
39f27f9d6a
commit
f7d1ace883
|
@ -68,13 +68,6 @@ class LanePlanner:
|
|||
self.r_poly[3] += CAMERA_OFFSET
|
||||
|
||||
# Find current lanewidth
|
||||
self.lane_width_certainty += 0.05 * (self.l_prob * self.r_prob - self.lane_width_certainty)
|
||||
current_lane_width = abs(self.l_poly[3] - self.r_poly[3])
|
||||
self.lane_width_estimate += 0.005 * (current_lane_width - self.lane_width_estimate)
|
||||
speed_lane_width = interp(v_ego, [0., 31.], [2.8, 3.5])
|
||||
self.lane_width = self.lane_width_certainty * self.lane_width_estimate + \
|
||||
(1 - self.lane_width_certainty) * speed_lane_width
|
||||
|
||||
# This will improve behaviour when lanes suddenly widen
|
||||
# these numbers were tested on 2000 segments and found to work well
|
||||
l_prob, r_prob = self.l_prob, self.r_prob
|
||||
|
@ -94,6 +87,13 @@ class LanePlanner:
|
|||
l_prob *= l_std_mod
|
||||
r_prob *= r_std_mod
|
||||
|
||||
self.lane_width_certainty += 0.05 * (l_prob * r_prob - self.lane_width_certainty)
|
||||
current_lane_width = abs(self.l_poly[3] - self.r_poly[3])
|
||||
self.lane_width_estimate += 0.005 * (current_lane_width - self.lane_width_estimate)
|
||||
speed_lane_width = interp(v_ego, [0., 31.], [2.8, 3.5])
|
||||
self.lane_width = self.lane_width_certainty * self.lane_width_estimate + \
|
||||
(1 - self.lane_width_certainty) * speed_lane_width
|
||||
|
||||
clipped_lane_width = min(4.0, self.lane_width)
|
||||
path_from_left_lane = self.l_poly.copy()
|
||||
path_from_left_lane[3] -= clipped_lane_width / 2.0
|
||||
|
|
|
@ -1 +1 @@
|
|||
2d717d808d568c2a3939627430fc56747c52e5bd
|
||||
b4a8c7c439c2430082ffb49ed675ada8f3bab8e6
|
Loading…
Reference in a new issue