UI: refactor model related functions (#19657)

Squashed commit of the following:

commit 7d7fb15e4cdd0e47fa38f949f79581dc611db015
Author: Willem Melching <willem.melching@gmail.com>
Date:   Wed Jan 6 11:31:09 2021 +0100

    no extern

commit 901893966e
Merge: 9816e01e c0703a57
Author: Willem Melching <willem.melching@gmail.com>
Date:   Wed Jan 6 11:27:29 2021 +0100

    Merge branch 'master' into refactor_model_draw

commit 9816e01e6f
Author: deanlee <deanlee3@gmail.com>
Date:   Wed Jan 6 04:02:08 2021 +0000

    fix lead_d

commit 6fa500dbce
Author: deanlee <deanlee3@gmail.com>
Date:   Tue Jan 5 13:18:20 2021 +0000

    populate model data in update_socket

commit e25db7e28c
Author: deanlee <deanlee3@gmail.com>
Date:   Tue Jan 5 12:59:13 2021 +0000

    update model in function

commit 0834a3ffe7
Author: deanlee <deanlee3@gmail.com>
Date:   Tue Jan 5 12:50:21 2021 +0000

    continue

commit 95b08679c2
Author: Willem Melching <willem.melching@gmail.com>
Date:   Tue Jan 5 13:26:34 2021 +0100

    Update selfdrive/ui/paint.cc

commit 0eef065ae4
Author: Dean Lee <deanlee3@gmail.com>
Date:   Tue Jan 5 20:11:01 2021 +0800

    Update selfdrive/ui/paint.cc

    Co-authored-by: Willem Melching <willem.melching@gmail.com>

commit 955ad9b03a
Author: deanlee <deanlee3@gmail.com>
Date:   Thu Dec 31 04:45:31 2020 +0000

    refactor model related functions

old-commit-hash: 37cb4bec63

selfdrive/ui/paint.cc

@@ -41,7 +41,7 @@ const mat3 intrinsic_matrix = (mat3){{
 
 // Projects a point in car to space to the corresponding point in full frame
 // image space.
-bool car_space_to_full_frame(const UIState *s, float in_x, float in_y, float in_z, float *out_x, float *out_y, float margin=0.0) {
+bool car_space_to_full_frame(const UIState *s, float in_x, float in_y, float in_z, float *out_x, float *out_y, float margin) {
   const vec4 car_space_projective = (vec4){{in_x, in_y, in_z, 1.}};
   // We'll call the car space point p.
   // First project into normalized image coordinates with the extrinsics matrix.
@@ -146,35 +146,6 @@ static void ui_draw_line(UIState *s, const vertex_data *v, const int cnt, NVGcol
   nvgFill(s->vg);
 }
 
-static void update_track_data(UIState *s, const cereal::ModelDataV2::XYZTData::Reader &line, track_vertices_data *pvd) {
-  const UIScene *scene = &s->scene;
-  const float off = 0.5;
-  int max_idx = -1;
-  const auto lead = scene->lead_data[0];
-  float lead_d = lead.getStatus() ? lead.getDRel()*2. : MAX_DRAW_DISTANCE;
-
-  float path_length = (lead_d>0.)?lead_d-fmin(lead_d*0.35, 10.):MAX_DRAW_DISTANCE;
-  path_length = fmin(path_length, scene->max_distance);
-
-
-  vertex_data *v = &pvd->v[0];
-  const float margin = 500.0f;
-  for (int i = 0; i < TRAJECTORY_SIZE and line.getX()[i] <= path_length; i++) {
-    v += car_space_to_full_frame(s, line.getX()[i], -line.getY()[i] - off, -line.getZ()[i], &v->x, &v->y, margin);
-    max_idx = i;
-  }
-  for (int i = max_idx; i >= 0; i--) {
-    v += car_space_to_full_frame(s, line.getX()[i], -line.getY()[i] + off, -line.getZ()[i], &v->x, &v->y, margin);
-  }
-  pvd->cnt = v - pvd->v;
-}
-
-static void ui_draw_track(UIState *s, track_vertices_data *pvd) {
-  NVGpaint track_bg = nvgLinearGradient(s->vg, s->fb_w, s->fb_h, s->fb_w, s->fb_h * .4,
-                                        COLOR_WHITE, COLOR_WHITE_ALPHA(0));
-  ui_draw_line(s, &pvd->v[0], pvd->cnt, nullptr, &track_bg);
-}
-
 static void draw_frame(UIState *s) {
   mat4 *out_mat;
   if (s->scene.frontview) {
@@ -206,49 +177,24 @@ static void draw_frame(UIState *s) {
   glBindVertexArray(0);
 }
 
-static void update_line_data(UIState *s, const cereal::ModelDataV2::XYZTData::Reader &line, float off, line_vertices_data *pvd, float max_distance) {
-  // TODO check that this doesn't overflow max vertex buffer
-  int max_idx = -1;
-  vertex_data *v = &pvd->v[0];
-  const float margin = 500.0f;
-  for (int i = 0; ((i < TRAJECTORY_SIZE) and (line.getX()[i] < fmax(MIN_DRAW_DISTANCE, max_distance))); i++) {
-    v += car_space_to_full_frame(s, line.getX()[i], -line.getY()[i] - off, -line.getZ()[i] + 1.22, &v->x, &v->y, margin);
-    max_idx = i;
-  }
-  for (int i = max_idx; i >= 0; i--) {
-    v += car_space_to_full_frame(s, line.getX()[i], -line.getY()[i] + off, -line.getZ()[i] + 1.22, &v->x, &v->y, margin);
-  }
-  pvd->cnt = v - pvd->v;
-}
-
 static void ui_draw_vision_lane_lines(UIState *s) {
-  const UIScene *scene = &s->scene;
+  const UIScene &scene = s->scene;
-
   // paint lanelines
-  line_vertices_data *pvd_ll = &s->lane_line_vertices[0];
+  for (int i = 0; i < std::size(scene.lane_line_vertices); i++) {
-  for (int ll_idx = 0; ll_idx < 4; ll_idx++) {
+    NVGcolor color = nvgRGBAf(1.0, 1.0, 1.0, scene.lane_line_probs[i]);
-    if (s->sm->updated("modelV2")) {
+    ui_draw_line(s, scene.lane_line_vertices[i].v, scene.lane_line_vertices[i].cnt, &color, nullptr);
-      update_line_data(s, scene->model.getLaneLines()[ll_idx], 0.025*scene->model.getLaneLineProbs()[ll_idx], pvd_ll + ll_idx, scene->max_distance);
-    }
-    NVGcolor color = nvgRGBAf(1.0, 1.0, 1.0, scene->lane_line_probs[ll_idx]);
-    ui_draw_line(s, (pvd_ll + ll_idx)->v, (pvd_ll + ll_idx)->cnt, &color, nullptr);
   }
 
   // paint road edges
-  line_vertices_data *pvd_re = &s->road_edge_vertices[0];
+  for (int i = 0; i < std::size(scene.road_edge_vertices); i++) {
-  for (int re_idx = 0; re_idx < 2; re_idx++) {
+    NVGcolor color = nvgRGBAf(1.0, 0.0, 0.0, std::clamp<float>(1.0 - scene.road_edge_stds[i], 0.0, 1.0));
-    if (s->sm->updated("modelV2")) {
+    ui_draw_line(s, scene.road_edge_vertices[i].v, scene.road_edge_vertices[i].cnt, &color, nullptr);
-      update_line_data(s, scene->model.getRoadEdges()[re_idx], 0.025, pvd_re + re_idx, scene->max_distance);
-    }
-    NVGcolor color = nvgRGBAf(1.0, 0.0, 0.0, std::clamp<float>(1.0-scene->road_edge_stds[re_idx], 0.0, 1.0));
-    ui_draw_line(s, (pvd_re + re_idx)->v, (pvd_re + re_idx)->cnt, &color, nullptr);
   }
 
   // paint path
-  if (s->sm->updated("modelV2")) {
+  NVGpaint track_bg = nvgLinearGradient(s->vg, s->fb_w, s->fb_h, s->fb_w, s->fb_h * .4,
-    update_track_data(s, scene->model.getPosition(), &s->track_vertices);
+                                        COLOR_WHITE, COLOR_WHITE_ALPHA(0));
-  }
+  ui_draw_line(s, scene.track_vertices.v, scene.track_vertices.cnt, nullptr, &track_bg);
-  ui_draw_track(s, &s->track_vertices);
 }
 
 // Draw all world space objects.

selfdrive/ui/paint.hpp

@@ -1,6 +1,7 @@
 #pragma once
 #include "ui.hpp"
 
+bool car_space_to_full_frame(const UIState *s, float in_x, float in_y, float in_z, float *out_x, float *out_y, float margin=0.0);
 void ui_draw(UIState *s);
 void ui_draw_image(NVGcontext *vg, const Rect &r, int image, float alpha);
 void ui_draw_rect(NVGcontext *vg, float x, float y, float w, float h, NVGcolor color, float r = 0, int width = 0);

selfdrive/ui/ui.cc

@@ -16,6 +16,7 @@
 #include "ui.hpp"
 #include "paint.hpp"
 
+
 int write_param_float(float param, const char* param_name, bool persistent_param) {
   char s[16];
   int size = snprintf(s, sizeof(s), "%f", param);
@@ -104,6 +105,50 @@ destroy:
   s->vision_connected = false;
 }
 
+template <class T>
+static void update_line_data(const UIState *s, const cereal::ModelDataV2::XYZTData::Reader &line,
+                             float y_off, float z_off, T *pvd, float max_distance) {
+  const auto line_x = line.getX(), line_y = line.getY(), line_z = line.getZ();
+  int max_idx = -1;
+  vertex_data *v = &pvd->v[0];
+  const float margin = 500.0f;
+  for (int i = 0; ((i < TRAJECTORY_SIZE) and (line_x[i] < fmax(MIN_DRAW_DISTANCE, max_distance))); i++) {
+    v += car_space_to_full_frame(s, line_x[i], -line_y[i] - y_off, -line_z[i] + z_off, &v->x, &v->y, margin);
+    max_idx = i;
+  }
+  for (int i = max_idx; i >= 0; i--) {
+    v += car_space_to_full_frame(s, line_x[i], -line_y[i] + y_off, -line_z[i] + z_off, &v->x, &v->y, margin);
+  }
+  pvd->cnt = v - pvd->v;
+  assert(pvd->cnt < std::size(pvd->v));
+}
+
+static void update_model(UIState *s, const cereal::ModelDataV2::Reader &model) {
+  UIScene &scene = s->scene;
+  const float max_distance = fmin(model.getPosition().getX()[TRAJECTORY_SIZE - 1], MAX_DRAW_DISTANCE);
+  // update lane lines
+  const auto lane_lines = model.getLaneLines();
+  const auto lane_line_probs = model.getLaneLineProbs();
+  for (int i = 0; i < std::size(scene.lane_line_vertices); i++) {
+    scene.lane_line_probs[i] = lane_line_probs[i];
+    update_line_data(s, lane_lines[i], 0.025 * scene.lane_line_probs[i], 1.22, &scene.lane_line_vertices[i], max_distance);
+  }
+
+  // update road edges
+  const auto road_edges = model.getRoadEdges();
+  const auto road_edge_stds = model.getRoadEdgeStds();
+  for (int i = 0; i < std::size(scene.road_edge_vertices); i++) {
+    scene.road_edge_stds[i] = road_edge_stds[i];
+    update_line_data(s, road_edges[i], 0.025, 1.22, &scene.road_edge_vertices[i], max_distance);
+  }
+
+  // update path
+  const float lead_d = scene.lead_data[0].getStatus() ? scene.lead_data[0].getDRel() * 2. : MAX_DRAW_DISTANCE;
+  float path_length = (lead_d > 0.) ? lead_d - fmin(lead_d * 0.35, 10.) : MAX_DRAW_DISTANCE;
+  path_length = fmin(path_length, max_distance);
+  update_line_data(s, model.getPosition(), 0.5, 0, &scene.track_vertices, path_length);
+}
+
 void update_sockets(UIState *s) {
 
   UIScene &scene = s->scene;
@@ -170,23 +215,7 @@ void update_sockets(UIState *s) {
     }
   }
   if (sm.updated("modelV2")) {
-    scene.model = sm["modelV2"].getModelV2();
+    update_model(s, sm["modelV2"].getModelV2());
-    scene.max_distance = fmin(scene.model.getPosition().getX()[TRAJECTORY_SIZE - 1], MAX_DRAW_DISTANCE);
-    for (int ll_idx = 0; ll_idx < 4; ll_idx++) {
-      if (scene.model.getLaneLineProbs().size() > ll_idx) {
-        scene.lane_line_probs[ll_idx] = scene.model.getLaneLineProbs()[ll_idx];
-      } else {
-        scene.lane_line_probs[ll_idx] = 0.0;
-      }
-    }
-
-    for (int re_idx = 0; re_idx < 2; re_idx++) {
-      if (scene.model.getRoadEdgeStds().size() > re_idx) {
-        scene.road_edge_stds[re_idx] = scene.model.getRoadEdgeStds()[re_idx];
-      } else {
-        scene.road_edge_stds[re_idx] = 1.0;
-      }
-    }
   }
   if (sm.updated("uiLayoutState")) {
     auto data = sm["uiLayoutState"].getUiLayoutState();

selfdrive/ui/ui.hpp

@@ -86,11 +86,18 @@ static std::map<UIStatus, NVGcolor> bg_colors = {
 };
 
 typedef struct {
-  float x[TRAJECTORY_SIZE];
+  float x, y;
-  float y[TRAJECTORY_SIZE];
+} vertex_data;
-  float z[TRAJECTORY_SIZE];
-} line;
 
+typedef struct {
+  vertex_data v[MODEL_PATH_MAX_VERTICES_CNT];
+  int cnt;
+} line_vertices_data;
+
+typedef struct {
+  vertex_data v[TRACK_POINTS_MAX_CNT];
+  int cnt;
+} track_vertices_data;
 
 typedef struct UIScene {
 
@@ -117,34 +124,15 @@ typedef struct UIScene {
   cereal::ControlsState::Reader controls_state;
   cereal::DriverState::Reader driver_state;
   cereal::DMonitoringState::Reader dmonitoring_state;
-  cereal::ModelDataV2::Reader model;
+
-  line path;
+  // modelV2
-  line outer_left_lane_line;
-  line left_lane_line;
-  line right_lane_line;
-  line outer_right_lane_line;
-  line left_road_edge;
-  line right_road_edge;
-  float max_distance;
   float lane_line_probs[4];
   float road_edge_stds[2];
+  track_vertices_data track_vertices;
+  line_vertices_data lane_line_vertices[4];
+  line_vertices_data road_edge_vertices[2];
 } UIScene;
 
-typedef struct {
-  float x, y;
-} vertex_data;
-
-typedef struct {
-  vertex_data v[MODEL_PATH_MAX_VERTICES_CNT];
-  int cnt;
-} line_vertices_data;
-
-typedef struct {
-  vertex_data v[TRACK_POINTS_MAX_CNT];
-  int cnt;
-} track_vertices_data;
-
-
 typedef struct UIState {
   // framebuffer
   FramebufferState *fb;
@@ -201,10 +189,6 @@ typedef struct UIState {
   bool alert_blinked;
   float alert_blinking_alpha;
 
-  track_vertices_data track_vertices;
-  line_vertices_data lane_line_vertices[4];
-  line_vertices_data road_edge_vertices[2];
-
   Rect video_rect;
 } UIState;