Lane Detection

frogpilot/common/frogpilot_utilities.py

@@ -85,6 +85,31 @@ def calculate_distance_to_point(lat1, lon1, lat2, lon2):
   return EARTH_RADIUS * c
 
 
+def calculate_lane_width(lane_line1, lane_line2, road_edge=None):
+  lane_line1_x = np.asarray(lane_line1.x)
+  lane_line1_y = np.asarray(lane_line1.y)
+
+  lane_line2_x = np.asarray(lane_line2.x)
+  lane_line2_y = np.asarray(lane_line2.y)
+
+  lane_y_interp = np.interp(lane_line2_x, lane_line1_x, lane_line1_y)
+  distance_to_lane = np.median(np.abs(lane_line2_y - lane_y_interp))
+
+  if road_edge is None:
+    return float(distance_to_lane)
+
+  edge_line_x = np.asarray(road_edge.x)
+  edge_line_y = np.asarray(road_edge.y)
+
+  edge_y_interp = np.interp(lane_line2_x, edge_line_x, edge_line_y)
+  distance_to_road_edge = np.median(np.abs(lane_line2_y - edge_y_interp))
+
+  if distance_to_road_edge < distance_to_lane:
+    return 0.0
+
+  return float(distance_to_lane)
+
+
 # Credit goes to Pfeiferj!
 def calculate_road_curvature(modelData):
   orientation_rate = np.array(modelData.orientationRate.z)

frogpilot/controls/frogpilot_planner.py

@@ -11,7 +11,7 @@ from openpilot.common.realtime import DT_MDL
 from openpilot.selfdrive.car.cruise import V_CRUISE_MAX
 from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import A_CHANGE_COST, DANGER_ZONE_COST, J_EGO_COST, STOP_DISTANCE
 
-from openpilot.frogpilot.common.frogpilot_utilities import calculate_road_curvature
+from openpilot.frogpilot.common.frogpilot_utilities import calculate_lane_width, calculate_road_curvature
 from openpilot.frogpilot.common.frogpilot_variables import CRUISING_SPEED, MINIMUM_LATERAL_ACCELERATION, PLANNER_TIME, THRESHOLD
 from openpilot.frogpilot.controls.lib.conditional_experimental_mode import ConditionalExperimentalMode
 from openpilot.frogpilot.controls.lib.frogpilot_acceleration import FrogPilotAcceleration
@@ -36,6 +36,8 @@ class FrogPilotPlanner:
     self.road_curvature_detected = False
     self.tracking_lead = False
 
+    self.lane_width_left = 0
+    self.lane_width_right = 0
     self.lateral_acceleration = 0
     self.model_length = 0
     self.road_curvature = 0
@@ -82,6 +84,13 @@ class FrogPilotPlanner:
     }
     self.params_memory.put("LastGPSPosition", json.dumps(self.gps_position))
 
+    if v_ego >= frogpilot_toggles.minimum_lane_change_speed:
+      self.lane_width_left = calculate_lane_width(sm["modelV2"].laneLines[0], sm["modelV2"].laneLines[1], sm["modelV2"].roadEdges[0])
+      self.lane_width_right = calculate_lane_width(sm["modelV2"].laneLines[3], sm["modelV2"].laneLines[2], sm["modelV2"].roadEdges[1])
+    else:
+      self.lane_width_left = 0
+      self.lane_width_right = 0
+
     self.lateral_acceleration = v_ego**2 * sm["controlsState"].curvature
 
     self.lateral_check |= sm["carState"].standstill
@@ -132,6 +141,9 @@ class FrogPilotPlanner:
 
     frogpilotPlan.increasedStoppedDistance = frogpilot_toggles.increase_stopped_distance
 
+    frogpilotPlan.laneWidthLeft = self.lane_width_left
+    frogpilotPlan.laneWidthRight = self.lane_width_right
+
     frogpilotPlan.lateralCheck = self.lateral_check
 
     frogpilotPlan.maxAcceleration = float(self.frogpilot_acceleration.max_accel)

frogpilot/ui/qt/onroad/frogpilot_annotated_camera.cc

@@ -156,6 +156,8 @@ void FrogPilotAnnotatedCameraWidget::updateState(const UIState &s, const FrogPil
   cscTraining = frogpilotPlan.getCscTraining();
   experimentalMode = selfdriveState.getExperimentalMode();
   forceCoast = frogpilotCarState.getForceCoast();
+  laneWidthLeft = frogpilotPlan.getLaneWidthLeft();
+  laneWidthRight = frogpilotPlan.getLaneWidthRight();
   redLight = frogpilotPlan.getRedLight();
   roadCurvature = frogpilotPlan.getRoadCurvature();
   roadName = QString::fromStdString(params_memory.get("RoadName"));
@@ -256,6 +258,63 @@ void FrogPilotAnnotatedCameraWidget::paintFrogPilotWidgets(QPainter &p, UIState
   }
 }
 
+void FrogPilotAnnotatedCameraWidget::paintAdjacentPaths(QPainter &p) {
+  std::function<void(const QPolygonF&, bool, bool, float)> paintPath = [&](const QPolygonF &path, bool isLeft, bool isBlindSpot, float laneWidth) {
+    if (laneWidth == 0.0f) {
+      return;
+    }
+
+    p.save();
+
+    QLinearGradient gradient(0, height(), 0, 0);
+    if (isBlindSpot && frogpilot_toggles.value("blind_spot_path").toBool()) {
+      gradient.setColorAt(0.0f, QColor::fromHslF(0.0f, 0.75f, 0.5f, 0.4f));
+      gradient.setColorAt(0.5f, QColor::fromHslF(0.0f, 0.75f, 0.5f, 0.35f));
+      gradient.setColorAt(1.0f, QColor::fromHslF(0.0f, 0.75f, 0.5f, 0.0f));
+    } else {
+      float ratio = std::clamp(laneWidth / frogpilot_toggles.value("lane_detection_width").toDouble(), 0.0, 1.0);
+      float hue = (ratio * ratio) * (120.0f / 360.0f);
+
+      gradient.setColorAt(0.0f, QColor::fromHslF(hue, 0.75f, 0.5f, 0.4f));
+      gradient.setColorAt(0.5f, QColor::fromHslF(hue, 0.75f, 0.5f, 0.35f));
+      gradient.setColorAt(1.0f, QColor::fromHslF(hue, 0.75f, 0.5f, 0.0f));
+    }
+
+    p.setBrush(gradient);
+    p.drawPolygon(path);
+
+    if (frogpilot_toggles.value("adjacent_path_metrics").toBool()) {
+      QString text;
+      if (isBlindSpot && frogpilot_toggles.value("blind_spot_path").toBool()) {
+        text = tr("Vehicle in blind spot");
+      } else {
+        text = QString::number(laneWidth * distanceConversion, 'f', 2) + leadDistanceUnit;
+      }
+
+      int midIndex = path.size() / 2;
+      QPointF anchorPoint = isLeft ? path[midIndex / 2] : path[midIndex + (path.size() - midIndex) / 2];
+
+      p.setFont(InterFont(45, QFont::DemiBold));
+      QFontMetrics metrics(p.font());
+
+      int textXPosition = isLeft ? anchorPoint.x() - metrics.horizontalAdvance(text) : anchorPoint.x();
+      int textYPosition = anchorPoint.y() - metrics.height() / 2 + metrics.ascent();
+
+      QPainterPath textPath;
+      textPath.addText(textXPosition, textYPosition, p.font(), text);
+      p.strokePath(textPath, QPen(Qt::black, 3, Qt::SolidLine, Qt::RoundCap, Qt::RoundJoin));
+
+      p.setPen(whiteColor());
+      p.drawText(textXPosition, textYPosition, text);
+    }
+
+    p.restore();
+  };
+
+  paintPath(track_adjacent_vertices[0], true, blindspotLeft, laneWidthLeft);
+  paintPath(track_adjacent_vertices[1], false, blindspotRight, laneWidthRight);
+}
+
 void FrogPilotAnnotatedCameraWidget::paintBlindSpotPath(QPainter &p) {
   p.save();
 

frogpilot/ui/qt/onroad/frogpilot_annotated_camera.h

@@ -12,6 +12,7 @@ public:
   explicit FrogPilotAnnotatedCameraWidget(QWidget *parent = 0);
 
   void mousePressEvent(QMouseEvent *mouseEvent) override;
+  void paintAdjacentPaths(QPainter &p);
   void paintBlindSpotPath(QPainter &p);
   void paintFrogPilotWidgets(QPainter &p, UIState &s);
   void updateState(const UIState &s, const FrogPilotUIState &fs);
@@ -38,6 +39,8 @@ public:
   QPoint dmIconPosition;
   QPoint experimentalButtonPosition;
 
+  QPolygonF track_adjacent_vertices[2];
+
   QRect setSpeedRect;
 
   QSize defaultSize;
@@ -83,6 +86,8 @@ private:
   float accelerationEgo;
   float cscSpeed;
   float distanceConversion;
+  float laneWidthLeft;
+  float laneWidthRight;
   float roadCurvature;
   float setSpeed;
   float speedConversion;

selfdrive/ui/qt/onroad/model.cc

@@ -31,7 +31,7 @@ void ModelRenderer::draw(QPainter &painter, const QRect &surface_rect) {
   const auto &radar_state = sm["radarState"].getRadarState();
   const auto &lead_one = radar_state.getLeadOne();
 
-  update_model(model, lead_one);
+  update_model(model, lead_one, surface_rect.height());
   drawLaneLines(painter);
   drawPath(painter, model, surface_rect.height());
 
@@ -85,7 +85,7 @@ void ModelRenderer::update_leads(const cereal::RadarState::Reader &radar_state,
   }
 }
 
-void ModelRenderer::update_model(const cereal::ModelDataV2::Reader &model, const cereal::RadarState::LeadData::Reader &lead) {
+void ModelRenderer::update_model(const cereal::ModelDataV2::Reader &model, const cereal::RadarState::LeadData::Reader &lead, float height) {
   const auto &model_position = model.getPosition();
   float max_distance = *(model_position.getX().end() - 1);
 
@@ -125,6 +125,9 @@ void ModelRenderer::update_model(const cereal::ModelDataV2::Reader &model, const
   SubMaster &fpsm = *(fs->sm);
 
   const cereal::FrogPilotPlan::Reader &frogpilotPlan = fpsm["frogpilotPlan"].getFrogpilotPlan();
+
+  mapAveragedLineToPolygon(lane_lines[0], lane_lines[1], frogpilotPlan.getLaneWidthLeft() / 2.0f, 0, &frogpilot_nvg->track_adjacent_vertices[0], max_idx, height, false);
+  mapAveragedLineToPolygon(lane_lines[2], lane_lines[3], frogpilotPlan.getLaneWidthRight() / 2.0f, 0, &frogpilot_nvg->track_adjacent_vertices[1], max_idx, height, false);
 }
 
 void ModelRenderer::drawLaneLines(QPainter &painter) {
@@ -190,7 +193,9 @@ void ModelRenderer::drawPath(QPainter &painter, const cereal::ModelDataV2::Reade
   painter.drawPolygon(track_vertices);
 
   // FrogPilot variables
-  if (frogpilot_toggles.value("blind_spot_path").toBool()) {
+  if (frogpilot_toggles.value("adjacent_paths").toBool() || frogpilot_toggles.value("adjacent_path_metrics").toBool()) {
+    frogpilot_nvg->paintAdjacentPaths(painter);
+  } else if (frogpilot_toggles.value("blind_spot_path").toBool()) {
     frogpilot_nvg->paintBlindSpotPath(painter);
   }
 }
@@ -305,6 +310,49 @@ void ModelRenderer::mapLineToPolygon(const cereal::XYZTData::Reader &line, float
 }
 
 // FrogPilot variables
+void ModelRenderer::mapAveragedLineToPolygon(const cereal::XYZTData::Reader &line1, const cereal::XYZTData::Reader &line2, float y_off, float z_off,
+                                             QPolygonF *pvd, int max_idx, float height, bool allow_invert) {
+  const auto line_x1 = line1.getX(), line_y1 = line1.getY(), line_z1 = line1.getZ();
+  const auto line_y2 = line2.getY();
+  QPointF left, right;
+  pvd->clear();
+  for (int i = 0; i <= max_idx; i++) {
+    // highly negative x positions  are drawn above the frame and cause flickering, clip to zy plane of camera
+    if (line_x1[i] < 0) continue;
+
+    bool l = mapToScreen(line_x1[i], ((line_y1[i] + line_y2[i]) / 2.0f) - y_off, line_z1[i] + z_off, &left);
+    bool r = mapToScreen(line_x1[i], ((line_y1[i] + line_y2[i]) / 2.0f) + y_off, line_z1[i] + z_off, &right);
+    if (l && r) {
+      // For wider lines the drawn polygon will "invert" when going over a hill and cause artifacts
+      if (!allow_invert && pvd->size() && left.y() > pvd->back().y()) {
+        continue;
+      }
+      pvd->push_back(left);
+      pvd->push_front(right);
+    }
+  }
+
+  // Ground the path
+  if (pvd->size() >= 4) {
+    int mid = pvd->size() / 2;
+
+    std::function<void(int, int)> extendToBottom = [&](int idx1, int idx2) {
+      QPointF &p0 = (*pvd)[idx1];
+      QPointF &p1 = (*pvd)[idx2];
+
+      float dy = p0.y() - p1.y();
+      if (std::abs(dy) > 0.1f) {
+        float slope = (p0.x() - p1.x()) / dy;
+        p0.setX(p0.x() + (height - p0.y()) * slope);
+        p0.setY(height);
+      }
+    };
+
+    extendToBottom(mid, mid + 1);
+    extendToBottom(mid - 1, mid - 2);
+  }
+}
+
 void ModelRenderer::updateAdjacentLeads(const cereal::FrogPilotRadarState::Reader &radar_state, const cereal::XYZTData::Reader &line) {
   for (int i = 0; i < 2; ++i) {
     const auto &lead_data = (i == 0) ? radar_state.getLeadLeft() : radar_state.getLeadRight();

selfdrive/ui/qt/onroad/model.h

@@ -26,7 +26,7 @@ private:
                         QPolygonF *pvd, int max_idx, bool allow_invert = true);
   void drawLead(QPainter &painter, const cereal::RadarState::LeadData::Reader &lead_data, const QPointF &vd, const QRect &surface_rect, QColor marker_color, bool adjacent=false);
   void update_leads(const cereal::RadarState::Reader &radar_state, const cereal::XYZTData::Reader &line);
-  void update_model(const cereal::ModelDataV2::Reader &model, const cereal::RadarState::LeadData::Reader &lead);
+  void update_model(const cereal::ModelDataV2::Reader &model, const cereal::RadarState::LeadData::Reader &lead, float height);
   void drawLaneLines(QPainter &painter);
   void drawPath(QPainter &painter, const cereal::ModelDataV2::Reader &model, int height);
   void updatePathGradient(QLinearGradient &bg);
@@ -47,6 +47,8 @@ private:
   QRectF clip_region;
 
   // FrogPilot variables
+  void mapAveragedLineToPolygon(const cereal::XYZTData::Reader &line1, const cereal::XYZTData::Reader &line2, float y_off, float z_off,
+                                QPolygonF *pvd, int max_idx, float height, bool allow_invert = true);
   void updateAdjacentLeads(const cereal::FrogPilotRadarState::Reader &radar_state, const cereal::XYZTData::Reader &line);
   void updateRadarTracks(const cereal::XYZTData::Reader &line);