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 <adeebshihadeh@gmail.com>pull/20047/head
parent
4085ee2114
commit
ab0456c0eb
|
@ -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')
|
||||
|
|
|
@ -141,7 +141,3 @@ Eigen::Vector3d ned_euler_from_ecef(ECEF ecef_init, Eigen::Vector3d ecef_pose){
|
|||
return {phi, theta, psi};
|
||||
}
|
||||
|
||||
|
||||
|
||||
int main(void){
|
||||
}
|
||||
|
|
|
@ -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',
|
||||
|
|
|
@ -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.
|
||||
|
|
|
@ -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);
|
||||
|
|
|
@ -1,3 +1,4 @@
|
|||
#include <iostream>
|
||||
#include <stdio.h>
|
||||
#include <cmath>
|
||||
#include <stdlib.h>
|
||||
|
@ -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")) {
|
||||
|
|
|
@ -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;
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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))
|
||||
|
|
Loading…
Reference in New Issue