No uncertain ll (#2495)

* initial, needs tune

* support metrics

* a little more

* wrong type

* fixup

* update refs
old-commit-hash: 26583b1b85cfe496300ab61310c3489b542ecead

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)

selfdrive/test/process_replay/ref_commit

@@ -1 +1 @@
-804ee1ffcfa0639782f1322874d89f0c8c412c50
+2d717d808d568c2a3939627430fc56747c52e5bd