diff --git a/selfdrive/ui/qt/onroad/annotated_camera.cc b/selfdrive/ui/qt/onroad/annotated_camera.cc index c7ffa9b410b713b..9a190fde50d13b4 100644 --- a/selfdrive/ui/qt/onroad/annotated_camera.cc +++ b/selfdrive/ui/qt/onroad/annotated_camera.cc @@ -134,20 +134,21 @@ void AnnotatedCameraWidget::initializeGL() { void AnnotatedCameraWidget::updateFrameMat() { CameraWidget::updateFrameMat(); - UIState *s = uiState(); - int w = width(), h = height(); - - s->fb_w = w; - s->fb_h = h; // Apply transformation such that video pixel coordinates match video // 1) Put (0, 0) in the middle of the video // 2) Apply same scaling as video // 3) Put (0, 0) in top left corner of video - s->car_space_transform.reset(); - s->car_space_transform.translate(w / 2 - x_offset, h / 2 - y_offset) - .scale(zoom, zoom) - .translate(-intrinsic_matrix.v[2], -intrinsic_matrix.v[5]); + mat3 video_transform = {{ + zoom, 0.0f, (width() / 2 - x_offset) - (intrinsic_matrix.v[2] * zoom), + 0.0f, zoom, (height() / 2 - y_offset) - (intrinsic_matrix.v[5] * zoom), + 0.0f, 0.0f, 1.0f + }}; + + UIState *s = uiState(); + auto calib_transform = matmul3(intrinsic_matrix, calibration); + s->car_space_transform = matmul3(video_transform, calib_transform); + s->clip_region = rect().adjusted(-500, -500, 500, 500); } void AnnotatedCameraWidget::drawLaneLines(QPainter &painter, const UIState *s) { diff --git a/selfdrive/ui/ui.cc b/selfdrive/ui/ui.cc index 6de0babc7b91043..0463a9959bd1198 100644 --- a/selfdrive/ui/ui.cc +++ b/selfdrive/ui/ui.cc @@ -3,6 +3,7 @@ #include #include #include +#include #include @@ -19,20 +20,18 @@ // Projects a point in car to space to the corresponding point in full frame // image space. static bool calib_frame_to_full_frame(const UIState *s, float in_x, float in_y, float in_z, QPointF *out) { - const float margin = 500.0f; - const QRectF clip_region{-margin, -margin, s->fb_w + 2 * margin, s->fb_h + 2 * margin}; - - const vec3 pt = (vec3){{in_x, in_y, in_z}}; - const vec3 Ep = matvecmul3(s->scene.wide_cam ? s->scene.view_from_wide_calib : s->scene.view_from_calib, pt); - const vec3 KEp = matvecmul3(s->scene.wide_cam ? ECAM_INTRINSIC_MATRIX : FCAM_INTRINSIC_MATRIX, Ep); - - // Project. - QPointF point = s->car_space_transform.map(QPointF{KEp.v[0] / KEp.v[2], KEp.v[1] / KEp.v[2]}); - if (clip_region.contains(point)) { - *out = point; - return true; + const vec3 pt = matvecmul3(s->car_space_transform, (vec3){{in_x, in_y, in_z}}); + if (pt.v[2] < std::numeric_limits::epsilon()) { + return false; } - return false; + + QPointF screen_point(pt.v[0] / pt.v[2], pt.v[1] / pt.v[2]); + if (!s->clip_region.contains(screen_point)) { + return false; + } + + *out = screen_point; + return true; } int get_path_length_idx(const cereal::XYZTData::Reader &line, const float path_height) { diff --git a/selfdrive/ui/ui.h b/selfdrive/ui/ui.h index 4efdad8ac0d1109..f4e5f64c0edd9cd 100644 --- a/selfdrive/ui/ui.h +++ b/selfdrive/ui/ui.h @@ -8,7 +8,6 @@ #include #include #include -#include #include "cereal/messaging/messaging.h" #include "common/mat.h" @@ -117,16 +116,14 @@ class UIState : public QObject { inline PrimeType primeType() const { return prime_type; } inline bool hasPrime() const { return prime_type > PrimeType::PRIME_TYPE_NONE; } - int fb_w = 0, fb_h = 0; - std::unique_ptr sm; - UIStatus status; UIScene scene = {}; QString language; - QTransform car_space_transform; + mat3 car_space_transform; + QRectF clip_region; signals: void uiUpdate(const UIState &s);