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>
old-commit-hash: ab0456c0eb

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')

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){
-}

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',

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.

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

selfdrive/ui/ui.cc

@@ -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")) {

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;

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

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