mawei/MoreTore
1
0
Fork 0
mirror of https://github.com/MoreTore/openpilot.git synced 2026-08-05 16:26:14 +08:00
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 901893966e3f908435bceb8777253dfa916a3d91
Merge: 9816e01e d8e864e3
Author: Willem Melching <willem.melching@gmail.com>
Date:   Wed Jan 6 11:27:29 2021 +0100

    Merge branch 'master' into refactor_model_draw

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

    fix lead_d

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

    populate model data in update_socket

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

    update model in function

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

    continue

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

    Update selfdrive/ui/paint.cc

commit 0eef065ae4f96c2ee5d97b4ccdfdbf0ae4945385
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 955ad9b03a176174b86b28d5bb6f48d6e2a09281
Author: deanlee <deanlee3@gmail.com>
Date:   Thu Dec 31 04:45:31 2020 +0000

    refactor model related functions
This commit is contained in:
Willem Melching
2021-01-06 11:34:52 +01:00
parent d8e864e33d
commit 37cb4bec63
4 changed files with 74 additions and 114 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/ui/paint.cc
+11 -65
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.
selfdrive/ui/paint.hpp
+1
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);
selfdrive/ui/ui.cc
+46 -17
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();
selfdrive/ui/ui.hpp
+16 -32
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;