sadmen
/
openpilot_comma
mirror of https://github.com/commaai/openpilot.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

UI: refactor model related functions (#19657)

Browse Source 
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
commatwo_master
Willem Melching 4 years ago
parent b56d44bbd2
commit 1cc43f8d7d
4 changed files with 74 additions and 114 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 76
      selfdrive/ui/paint.cc
  2. 1
      selfdrive/ui/paint.hpp
  3. 63
      selfdrive/ui/ui.cc
  4. 48
      selfdrive/ui/ui.hpp

76
selfdrive/ui/paint.cc
Unescape View File

@ -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 ll_idx = 0; ll_idx < 4; ll_idx++) {
if (s->sm->updated("modelV2")) {
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);
for (int i = 0; i < std::size(scene.lane_line_vertices); i++) {
NVGcolor color = nvgRGBAf(1.0, 1.0, 1.0, scene.lane_line_probs[i]);
ui_draw_line(s, scene.lane_line_vertices[i].v, scene.lane_line_vertices[i].cnt, &color, nullptr);
}
// paint road edges
line_vertices_data *pvd_re = &s->road_edge_vertices[0];
for (int re_idx = 0; re_idx < 2; re_idx++) {
if (s->sm->updated("modelV2")) {
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);
for (int i = 0; i < std::size(scene.road_edge_vertices); i++) {
NVGcolor color = nvgRGBAf(1.0, 0.0, 0.0, std::clamp<float>(1.0 - scene.road_edge_stds[i], 0.0, 1.0));
ui_draw_line(s, scene.road_edge_vertices[i].v, scene.road_edge_vertices[i].cnt, &color, nullptr);
}
// paint path
if (s->sm->updated("modelV2")) {
update_track_data(s, scene->model.getPosition(), &s->track_vertices);
}
ui_draw_track(s, &s->track_vertices);
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, scene.track_vertices.v, scene.track_vertices.cnt, nullptr, &track_bg);
}
// Draw all world space objects.

1
selfdrive/ui/paint.hpp
Unescape View File

@ -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);

63
selfdrive/ui/ui.cc
Unescape View File

@ -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();
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;
}
}
update_model(s, sm["modelV2"].getModelV2());
}
if (sm.updated("uiLayoutState")) {
auto data = sm["uiLayoutState"].getUiLayoutState();

48
selfdrive/ui/ui.hpp
Unescape View File

@ -86,11 +86,18 @@ static std::map<UIStatus, NVGcolor> bg_colors = {
};
typedef struct {
float x[TRAJECTORY_SIZE];
float y[TRAJECTORY_SIZE];
float z[TRAJECTORY_SIZE];
} line;
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 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;
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;
// modelV2
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;

Write Preview
Loading…
Cancel
Save