From ab0456c0eb41090fd9292d8643975e93983ae85e Mon Sep 17 00:00:00 2001 From: HaraldSchafer Date: Tue, 9 Feb 2021 17:23:46 -0800 Subject: [PATCH] fixup ui (#20049) * fixup ui * works * new formate * more cleanup * works I assume we need a library somehow * more readable * fix lead sign * put on road * transformations lib Co-authored-by: Adeeb Shihadeh --- common/transformations/SConscript | 5 +++- common/transformations/orientation.cc | 4 ---- selfdrive/ui/SConscript | 5 ++-- selfdrive/ui/paint.cc | 11 +++------ selfdrive/ui/paint.hpp | 2 +- selfdrive/ui/ui.cc | 30 ++++++++++++++++-------- selfdrive/ui/ui.hpp | 3 ++- tools/replay/lib/ui_helpers.py | 33 ++++++++++++++------------- tools/replay/ui.py | 4 ++-- 9 files changed, 53 insertions(+), 44 deletions(-) diff --git a/common/transformations/SConscript b/common/transformations/SConscript index adc9642a4..ee9b9a2b7 100644 --- a/common/transformations/SConscript +++ b/common/transformations/SConscript @@ -1,3 +1,6 @@ -Import('envCython') +Import('env', 'envCython') + +transformations = env.Library('transformations', ['orientation.cc', 'coordinates.cc']) +Export('transformations') envCython.Program('transformations.so', 'transformations.pyx') diff --git a/common/transformations/orientation.cc b/common/transformations/orientation.cc index 9dbc2421c..8d1a304a2 100644 --- a/common/transformations/orientation.cc +++ b/common/transformations/orientation.cc @@ -141,7 +141,3 @@ Eigen::Vector3d ned_euler_from_ecef(ECEF ecef_init, Eigen::Vector3d ecef_pose){ return {phi, theta, psi}; } - - -int main(void){ -} diff --git a/selfdrive/ui/SConscript b/selfdrive/ui/SConscript index 22b50dfc8..1564084ad 100644 --- a/selfdrive/ui/SConscript +++ b/selfdrive/ui/SConscript @@ -1,9 +1,10 @@ import os -Import('env', 'qt_env', 'arch', 'common', 'messaging', 'gpucommon', 'visionipc', 'cereal') +Import('env', 'qt_env', 'arch', 'common', 'messaging', 'gpucommon', 'visionipc', + 'cereal', 'transformations') src = ['ui.cc', 'paint.cc', 'sidebar.cc', '#phonelibs/nanovg/nanovg.c'] -libs = [gpucommon, common, 'zmq', 'capnp', 'kj', 'm', 'OpenCL', cereal, messaging, visionipc] +libs = [gpucommon, common, 'zmq', 'capnp', 'kj', 'm', 'OpenCL', cereal, messaging, visionipc, transformations] if qt_env is None: libs += ['EGL', 'GLESv3', 'gnustl_shared', 'log', 'utils', 'gui', 'hardware', diff --git a/selfdrive/ui/paint.cc b/selfdrive/ui/paint.cc index c0a069185..678aad7f5 100644 --- a/selfdrive/ui/paint.cc +++ b/selfdrive/ui/paint.cc @@ -37,15 +37,10 @@ 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, vertex_data *out) { +bool calib_frame_to_full_frame(const UIState *s, float in_x, float in_y, float in_z, vertex_data *out) { const float margin = 500.0f; - 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. - const vec4 Ep4 = matvecmul(s->scene.extrinsic_matrix, car_space_projective); - - // The last entry is zero because of how we store E (to use matvecmul). - const vec3 Ep = {{Ep4.v[0], Ep4.v[1], Ep4.v[2]}}; + const vec3 pt = (vec3){{in_x, in_y, in_z}}; + const vec3 Ep = matvecmul3(s->scene.view_from_calib, pt); const vec3 KEp = matvecmul3(intrinsic_matrix, Ep); // Project. diff --git a/selfdrive/ui/paint.hpp b/selfdrive/ui/paint.hpp index 082946492..4f63d3ef4 100644 --- a/selfdrive/ui/paint.hpp +++ b/selfdrive/ui/paint.hpp @@ -1,7 +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, vertex_data *out); +bool calib_frame_to_full_frame(const UIState *s, float in_x, float in_y, float in_z, vertex_data *out); void ui_draw(UIState *s); void ui_draw_image(const UIState *s, const Rect &r, const char *name, float alpha); void ui_draw_rect(NVGcontext *vg, const Rect &r, NVGcolor color, int width, float radius = 0); diff --git a/selfdrive/ui/ui.cc b/selfdrive/ui/ui.cc index d0375c630..130e04112 100644 --- a/selfdrive/ui/ui.cc +++ b/selfdrive/ui/ui.cc @@ -1,3 +1,4 @@ +#include #include #include #include @@ -71,7 +72,8 @@ static void update_lead(UIState *s, const cereal::RadarState::Reader &radar_stat lead_data = (idx == 0) ? radar_state.getLeadOne() : radar_state.getLeadTwo(); if (lead_data.getStatus()) { const int path_idx = get_path_length_idx(line, lead_data.getDRel()); - car_space_to_full_frame(s, lead_data.getDRel(), lead_data.getYRel(), -line.getZ()[path_idx], &s->scene.lead_vertices[idx]); + // negative because radarState uses left positive convention + calib_frame_to_full_frame(s, lead_data.getDRel(), -lead_data.getYRel(), line.getZ()[path_idx] + 1.22, &s->scene.lead_vertices[idx]); } } @@ -81,11 +83,11 @@ static void update_line_data(const UIState *s, const cereal::ModelDataV2::XYZTDa int max_idx = -1; vertex_data *v = &pvd->v[0]; 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); + v += calib_frame_to_full_frame(s, line_x[i], line_y[i] - y_off, line_z[i] + z_off, v); 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); + v += calib_frame_to_full_frame(s, line_x[i], line_y[i] + y_off, line_z[i] + z_off, v); } pvd->cnt = v - pvd->v; assert(pvd->cnt < std::size(pvd->v)); @@ -99,7 +101,7 @@ static void update_model(UIState *s, const cereal::ModelDataV2::Reader &model) { 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_line_data(s, lane_lines[i], 0.025 * scene.lane_line_probs[i], 0, &scene.lane_line_vertices[i], max_distance); } // update road edges @@ -107,14 +109,14 @@ static void update_model(UIState *s, const cereal::ModelDataV2::Reader &model) { 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_line_data(s, road_edges[i], 0.025, 0, &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); + update_line_data(s, model.getPosition(), 0.5, 1.22, &scene.track_vertices, path_length); } static void update_sockets(UIState *s) { @@ -136,9 +138,19 @@ static void update_sockets(UIState *s) { } if (sm.updated("liveCalibration")) { scene.world_objects_visible = true; - auto extrinsicl = sm["liveCalibration"].getLiveCalibration().getExtrinsicMatrix(); - for (int i = 0; i < 3 * 4; i++) { - scene.extrinsic_matrix.v[i] = extrinsicl[i]; + auto rpy_list = sm["liveCalibration"].getLiveCalibration().getRpyCalib(); + Eigen::Vector3d rpy; + rpy << rpy_list[0], rpy_list[1], rpy_list[2]; + Eigen::Matrix3d device_from_calib = euler2rot(rpy); + Eigen::Matrix3d view_from_device; + view_from_device << 0,1,0, + 0,0,1, + 1,0,0; + Eigen::Matrix3d view_from_calib = view_from_device * device_from_calib; + for (int i = 0; i < 3; i++) { + for (int j = 0; j < 3; j++) { + scene.view_from_calib.v[i*3 + j] = view_from_calib(i,j); + } } } if (sm.updated("modelV2")) { diff --git a/selfdrive/ui/ui.hpp b/selfdrive/ui/ui.hpp index f51ef9378..61810d573 100644 --- a/selfdrive/ui/ui.hpp +++ b/selfdrive/ui/ui.hpp @@ -25,6 +25,7 @@ #include "common/modeldata.h" #include "common/params.h" #include "common/glutil.h" +#include "common/transformations/orientation.hpp" #include "sound.hpp" #include "visionipc.h" #include "visionipc_client.h" @@ -95,7 +96,7 @@ typedef struct { typedef struct UIScene { - mat4 extrinsic_matrix; // Last row is 0 so we can use mat4. + mat3 view_from_calib; bool world_objects_visible; bool is_rhd; diff --git a/tools/replay/lib/ui_helpers.py b/tools/replay/lib/ui_helpers.py index ae83af069..a0644b74c 100644 --- a/tools/replay/lib/ui_helpers.py +++ b/tools/replay/lib/ui_helpers.py @@ -7,7 +7,8 @@ import numpy as np import pygame # pylint: disable=import-error from common.transformations.camera import (eon_f_frame_size, eon_f_focal_length, - tici_f_frame_size, tici_f_focal_length) + tici_f_frame_size, tici_f_focal_length, + get_view_frame_from_calib_frame) from selfdrive.config import UIParams as UP from selfdrive.config import RADAR_TO_CAMERA @@ -45,16 +46,15 @@ for width, height, focal in cams: METER_WIDTH = 20 class Calibration: - def __init__(self, num_px, extrinsic, intrinsic): - self.extrinsic = extrinsic + def __init__(self, num_px, rpy, intrinsic): self.intrinsic = intrinsic + self.extrinsics_matrix = get_view_frame_from_calib_frame(rpy[0], rpy[1], rpy[2], 0.0)[:,:3] self.zoom = _BB_TO_FULL_FRAME[num_px][0, 0] def car_space_to_ff(self, x, y, z): - ones = np.ones_like(x) - car_space_projective = np.column_stack((x, y, z, ones)).T + car_space_projective = np.column_stack((x, y, z)).T - ep = self.extrinsic.dot(car_space_projective) + ep = self.extrinsics_matrix.dot(car_space_projective) kep = self.intrinsic.dot(ep) return (kep[:-1, :] / kep[-1, :]).T @@ -78,15 +78,15 @@ def find_color(lidar_surface, color): return ret -def to_lid_pt(y, x): - px, py = -x * UP.lidar_zoom + UP.lidar_car_x, -y * UP.lidar_zoom + UP.lidar_car_y +def to_topdown_pt(y, x): + px, py = x * UP.lidar_zoom + UP.lidar_car_x, -y * UP.lidar_zoom + UP.lidar_car_y if px > 0 and py > 0 and px < UP.lidar_x and py < UP.lidar_y: return int(px), int(py) return -1, -1 def draw_path(path, color, img, calibration, top_down, lid_color=None, z_off=0): - x, y, z = np.asarray(path.x), -np.asarray(path.y), -np.asarray(path.z) + z_off + x, y, z = np.asarray(path.x), np.asarray(path.y), np.asarray(path.z) + z_off pts = calibration.car_space_to_bb(x, y, z) pts = np.round(pts).astype(int) @@ -95,7 +95,7 @@ def draw_path(path, color, img, calibration, top_down, lid_color=None, z_off=0): if lid_color is not None and top_down is not None: tcolor = find_color(top_down[0], lid_color) for i in range(len(x)): - px, py = to_lid_pt(x[i], y[i]) + px, py = to_topdown_pt(x[i], y[i]) if px != -1: top_down[1][px, py] = tcolor @@ -197,21 +197,21 @@ def plot_model(m, img, calibration, top_down): x_std, _, _, _ = lead.xyvaStd x -= RADAR_TO_CAMERA - _, py_top = to_lid_pt(x + x_std, -y) - px, py_bottom = to_lid_pt(x - x_std, -y) + _, py_top = to_topdown_pt(x + x_std, y) + px, py_bottom = to_topdown_pt(x - x_std, y) top_down[1][int(round(px - 4)):int(round(px + 4)), py_top:py_bottom] = find_color(top_down[0], YELLOW) for path, prob, _ in zip(m.laneLines, m.laneLineProbs, m.laneLineStds): color = (0, int(255 * prob), 0) - draw_path(path, color, img, calibration, top_down, YELLOW, 1.22) + draw_path(path, color, img, calibration, top_down, YELLOW) for edge, std in zip(m.roadEdges, m.roadEdgeStds): prob = max(1 - std, 0) color = (int(255 * prob), 0, 0) - draw_path(edge, color, img, calibration, top_down, RED, 1.22) + draw_path(edge, color, img, calibration, top_down, RED) color = (255, 0, 0) - draw_path(m.position, color, img, calibration, top_down, RED) + draw_path(m.position, color, img, calibration, top_down, RED, 1.22) def maybe_update_radar_points(lt, lid_overlay): @@ -221,7 +221,8 @@ def maybe_update_radar_points(lt, lid_overlay): for track in lt: ar_pts[track.trackId] = [track.dRel, track.yRel, track.vRel, track.aRel, track.oncoming, track.stationary] for ids, pt in ar_pts.items(): - px, py = to_lid_pt(pt[0], pt[1]) + # negative here since radar is left positive + px, py = to_topdown_pt(pt[0], -pt[1]) if px != -1: if pt[-1]: color = 240 diff --git a/tools/replay/ui.py b/tools/replay/ui.py index a84766650..d910dd573 100755 --- a/tools/replay/ui.py +++ b/tools/replay/ui.py @@ -165,8 +165,8 @@ def ui_thread(addr, frame_address): maybe_update_radar_points(sm['liveTracks'], top_down[1]) if sm.updated['liveCalibration'] and num_px: - extrinsic_matrix = np.asarray(sm['liveCalibration'].extrinsicMatrix).reshape(3, 4) - calibration = Calibration(num_px, extrinsic_matrix, intrinsic_matrix) + rpyCalib = np.asarray(sm['liveCalibration'].rpyCalib) + calibration = Calibration(num_px, rpyCalib, intrinsic_matrix) # *** blits *** pygame.surfarray.blit_array(camera_surface, img.swapaxes(0, 1))