diff --git a/selfdrive/controls/lib/lane_planner.py b/selfdrive/controls/lib/lane_planner.py index 5b9485a29a..1e39f66366 100644 --- a/selfdrive/controls/lib/lane_planner.py +++ b/selfdrive/controls/lib/lane_planner.py @@ -21,7 +21,7 @@ def eval_poly(poly, x): return poly[3] + poly[2]*x + poly[1]*x**2 + poly[0]*x**3 -def calc_d_poly(l_poly, r_poly, p_poly, l_prob, r_prob, lane_width, v_ego): +def calc_d_poly(l_poly, r_poly, p_poly, l_prob, r_prob, lane_width, v_ego, l_std=0.05, r_std=0.05): # This will improve behaviour when lanes suddenly widen # these numbers were tested on 2000segments and found to work well lane_width = min(4.0, lane_width) @@ -34,6 +34,13 @@ def calc_d_poly(l_poly, r_poly, p_poly, l_prob, r_prob, lane_width, v_ego): l_prob = mod * l_prob r_prob = mod * r_prob + # Remove reliance on uncertain lanelines + # these numbers were tested on 2000segments and found to work well + l_std_mod = interp(l_std, [.15, .3], [1.0, 0.0]) + l_prob = l_std_mod * l_prob + r_std_mod = interp(r_std, [.15, .3], [1.0, 0.0]) + r_prob = r_std_mod * r_prob + path_from_left_lane = l_poly.copy() path_from_left_lane[3] -= lane_width / 2.0 path_from_right_lane = r_poly.copy() @@ -59,6 +66,9 @@ class LanePlanner(): self.l_prob = 0. self.r_prob = 0. + self.l_std = 0. + self.r_std = 0. + self.l_lane_change_prob = 0. self.r_lane_change_prob = 0. @@ -68,7 +78,9 @@ class LanePlanner(): def parse_model(self, md): if len(md.leftLane.poly): self.l_poly = np.array(md.leftLane.poly) + self.l_std = float(md.leftLane.std) self.r_poly = np.array(md.rightLane.poly) + self.r_std = float(md.rightLane.std) self.p_poly = np.array(md.path.poly) else: self.l_poly = model_polyfit(md.leftLane.points, self._path_pinv) # left line @@ -94,7 +106,7 @@ class LanePlanner(): self.lane_width = self.lane_width_certainty * self.lane_width_estimate + \ (1 - self.lane_width_certainty) * speed_lane_width - self.d_poly = calc_d_poly(self.l_poly, self.r_poly, self.p_poly, self.l_prob, self.r_prob, self.lane_width, v_ego) + self.d_poly = calc_d_poly(self.l_poly, self.r_poly, self.p_poly, self.l_prob, self.r_prob, self.lane_width, v_ego, self.l_std, self.r_std) def update(self, v_ego, md): self.parse_model(md) diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index 85fdf04eb7..ec3ba0bcc9 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -804ee1ffcfa0639782f1322874d89f0c8c412c50 \ No newline at end of file +2d717d808d568c2a3939627430fc56747c52e5bd \ No newline at end of file