From 6f020fb178298d2dd9429466ede1551ebfea7e64 Mon Sep 17 00:00:00 2001 From: Willem Melching Date: Fri, 24 Jun 2022 15:18:25 +0200 Subject: [PATCH 1/7] compute x and y offsets using calibration --- selfdrive/ui/qt/onroad.cc | 11 ++++--- selfdrive/ui/qt/onroad.h | 2 +- selfdrive/ui/qt/widgets/cameraview.cc | 42 +++++++++++++++++---------- selfdrive/ui/qt/widgets/cameraview.h | 8 +++-- selfdrive/ui/ui.h | 2 -- 5 files changed, 41 insertions(+), 24 deletions(-) diff --git a/selfdrive/ui/qt/onroad.cc b/selfdrive/ui/qt/onroad.cc index 27594162369d05..d0f2ce7d9ee4ba 100644 --- a/selfdrive/ui/qt/onroad.cc +++ b/selfdrive/ui/qt/onroad.cc @@ -208,6 +208,7 @@ void NvgWindow::updateState(const UIState &s) { setProperty("engageable", cs.getEngageable() || cs.getEnabled()); setProperty("dmActive", sm["driverMonitoringState"].getDriverMonitoringState().getIsActiveMode()); } + CameraViewWidget::updateCalibration(s.scene.view_from_calib); } void NvgWindow::drawHud(QPainter &p) { @@ -399,10 +400,11 @@ void NvgWindow::initializeGL() { setBackgroundColor(bg_colors[STATUS_DISENGAGED]); } -void NvgWindow::updateFrameMat(int w, int h) { - CameraViewWidget::updateFrameMat(w, h); - +void NvgWindow::updateFrameMat() { UIState *s = uiState(); + CameraViewWidget::updateFrameMat(); + int w = width(), h = height(); + s->fb_w = w; s->fb_h = h; auto intrinsic_matrix = s->wide_camera ? ecam_intrinsic_matrix : fcam_intrinsic_matrix; @@ -414,8 +416,9 @@ void NvgWindow::updateFrameMat(int w, int h) { // 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, h / 2 + y_offset) + 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]); } diff --git a/selfdrive/ui/qt/onroad.h b/selfdrive/ui/qt/onroad.h index 4cd88fc9e3f642..dc1e69da2a1d8b 100644 --- a/selfdrive/ui/qt/onroad.h +++ b/selfdrive/ui/qt/onroad.h @@ -70,7 +70,7 @@ class NvgWindow : public CameraViewWidget { void paintGL() override; void initializeGL() override; void showEvent(QShowEvent *event) override; - void updateFrameMat(int w, int h) override; + void updateFrameMat() override; void drawLaneLines(QPainter &painter, const UIState *s); void drawLead(QPainter &painter, const cereal::ModelDataV2::LeadDataV3::Reader &lead_data, const QPointF &vd); void drawHud(QPainter &p); diff --git a/selfdrive/ui/qt/widgets/cameraview.cc b/selfdrive/ui/qt/widgets/cameraview.cc index 8666eb1d4e450e..998e01cbf93e68 100644 --- a/selfdrive/ui/qt/widgets/cameraview.cc +++ b/selfdrive/ui/qt/widgets/cameraview.cc @@ -6,6 +6,8 @@ #include #endif +#include + #include #include @@ -59,13 +61,6 @@ const char frame_fragment_shader[] = "}\n"; #endif -const mat4 device_transform = {{ - 1.0, 0.0, 0.0, 0.0, - 0.0, 1.0, 0.0, 0.0, - 0.0, 0.0, 1.0, 0.0, - 0.0, 0.0, 0.0, 1.0, -}}; - mat4 get_driver_view_transform(int screen_width, int screen_height, int stream_width, int stream_height) { const float driver_view_ratio = 2.0; const float yscale = stream_height * driver_view_ratio / stream_width; @@ -185,35 +180,52 @@ void CameraViewWidget::hideEvent(QHideEvent *event) { } } -void CameraViewWidget::updateFrameMat(int w, int h) { +void CameraViewWidget::updateFrameMat() { + int w = width(), h = height(); + if (zoomed_view) { if (stream_type == VISION_STREAM_DRIVER) { - frame_mat = matmul(device_transform, get_driver_view_transform(w, h, stream_width, stream_height)); + frame_mat = get_driver_view_transform(w, h, stream_width, stream_height); } else { auto intrinsic_matrix = stream_type == VISION_STREAM_WIDE_ROAD ? ecam_intrinsic_matrix : fcam_intrinsic_matrix; float zoom = ZOOM / intrinsic_matrix.v[0]; if (stream_type == VISION_STREAM_WIDE_ROAD) { zoom *= 0.5; } + + // Project point at "infinity" to compute x and y offsets + // to ensure this ends up in the middle of the screen + // TODO: use proper perspective transform? + const vec3 inf = {{1000., 0., 0.}}; + const vec3 Ep = matvecmul3(calibration, inf); + const vec3 Kep = matvecmul3(intrinsic_matrix, Ep); + x_offset = (Kep.v[0] / Kep.v[2] - intrinsic_matrix.v[2]) * zoom; + y_offset = (Kep.v[1] / Kep.v[2] - intrinsic_matrix.v[5]) * zoom; + qWarning() << x_offset << y_offset; + float zx = zoom * 2 * intrinsic_matrix.v[2] / width(); float zy = zoom * 2 * intrinsic_matrix.v[5] / height(); - const mat4 frame_transform = {{ - zx, 0.0, 0.0, 0.0, - 0.0, zy, 0.0, -y_offset / height() * 2, + zx, 0.0, 0.0, -x_offset / width() * 2, + 0.0, zy, 0.0, y_offset / height() * 2, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, }}; - frame_mat = matmul(device_transform, frame_transform); + frame_mat = frame_transform; } } else if (stream_width > 0 && stream_height > 0) { // fit frame to widget size float widget_aspect_ratio = (float)width() / height(); float frame_aspect_ratio = (float)stream_width / stream_height; - frame_mat = matmul(device_transform, get_fit_view_transform(widget_aspect_ratio, frame_aspect_ratio)); + frame_mat = get_fit_view_transform(widget_aspect_ratio, frame_aspect_ratio); } } +void CameraViewWidget::updateCalibration(const mat3 &calib) { + calibration = calib; + updateFrameMat(); +} + void CameraViewWidget::paintGL() { glClearColor(bg.redF(), bg.greenF(), bg.blueF(), bg.alphaF()); glClear(GL_STENCIL_BUFFER_BIT | GL_COLOR_BUFFER_BIT); @@ -311,7 +323,7 @@ void CameraViewWidget::vipcConnected(VisionIpcClient *vipc_client) { assert(glGetError() == GL_NO_ERROR); #endif - updateFrameMat(width(), height()); + updateFrameMat(); } void CameraViewWidget::vipcFrameReceived(VisionBuf *buf, uint32_t frame_id) { diff --git a/selfdrive/ui/qt/widgets/cameraview.h b/selfdrive/ui/qt/widgets/cameraview.h index 42e9043602000f..141aea6f8d3f48 100644 --- a/selfdrive/ui/qt/widgets/cameraview.h +++ b/selfdrive/ui/qt/widgets/cameraview.h @@ -42,11 +42,12 @@ class CameraViewWidget : public QOpenGLWidget, protected QOpenGLFunctions { protected: void paintGL() override; void initializeGL() override; - void resizeGL(int w, int h) override { updateFrameMat(w, h); } + void resizeGL(int w, int h) override { updateFrameMat(); } void showEvent(QShowEvent *event) override; void hideEvent(QHideEvent *event) override; void mouseReleaseEvent(QMouseEvent *event) override { emit clicked(); } - virtual void updateFrameMat(int w, int h); + virtual void updateFrameMat(); + void updateCalibration(const mat3 &calib); void vipcThread(); bool zoomed_view; @@ -65,6 +66,9 @@ class CameraViewWidget : public QOpenGLWidget, protected QOpenGLFunctions { int stream_width = 0; int stream_height = 0; int stream_stride = 0; + float x_offset = 0; + float y_offset = 0; + mat3 calibration = {{1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 1.0}}; std::atomic stream_type; QThread *vipc_thread = nullptr; diff --git a/selfdrive/ui/ui.h b/selfdrive/ui/ui.h index cbf3921e4ebefa..d3a69afb41482e 100644 --- a/selfdrive/ui/ui.h +++ b/selfdrive/ui/ui.h @@ -22,9 +22,7 @@ const int footer_h = 280; const int UI_FREQ = 20; // Hz typedef cereal::CarControl::HUDControl::AudibleAlert AudibleAlert; -// TODO: this is also hardcoded in common/transformations/camera.py // TODO: choose based on frame input size -const float y_offset = 150.0; const float ZOOM = 2912.8; struct Alert { From 83c1212a03e9686d3587f5855af8b20d1e8e8952 Mon Sep 17 00:00:00 2001 From: Willem Melching Date: Fri, 24 Jun 2022 15:26:45 +0200 Subject: [PATCH 2/7] fix default calibration --- selfdrive/ui/qt/widgets/cameraview.h | 6 +++++- selfdrive/ui/ui.h | 6 +++++- 2 files changed, 10 insertions(+), 2 deletions(-) diff --git a/selfdrive/ui/qt/widgets/cameraview.h b/selfdrive/ui/qt/widgets/cameraview.h index 141aea6f8d3f48..3f2a5d20bddaf0 100644 --- a/selfdrive/ui/qt/widgets/cameraview.h +++ b/selfdrive/ui/qt/widgets/cameraview.h @@ -68,7 +68,11 @@ class CameraViewWidget : public QOpenGLWidget, protected QOpenGLFunctions { int stream_stride = 0; float x_offset = 0; float y_offset = 0; - mat3 calibration = {{1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 1.0}}; + mat3 calibration = {{ + 0.0, 1.0, 0.0, + 0.0, 0.0, 1.0, + 1.0, 0.0, 0.0 + }}; std::atomic stream_type; QThread *vipc_thread = nullptr; diff --git a/selfdrive/ui/ui.h b/selfdrive/ui/ui.h index d3a69afb41482e..b4c86fab10822e 100644 --- a/selfdrive/ui/ui.h +++ b/selfdrive/ui/ui.h @@ -91,7 +91,11 @@ typedef struct { } line_vertices_data; typedef struct UIScene { - mat3 view_from_calib; + mat3 view_from_calib = {{ + 0.0, 1.0, 0.0, + 0.0, 0.0, 1.0, + 1.0, 0.0, 0.0 + }}; cereal::PandaState::PandaType pandaType; // modelV2 From 865c6e9116d73c2d0895862605821553c7ee9683 Mon Sep 17 00:00:00 2001 From: Willem Melching Date: Fri, 24 Jun 2022 15:42:05 +0200 Subject: [PATCH 3/7] clamp to max values --- selfdrive/ui/qt/widgets/cameraview.cc | 12 +++++++++--- 1 file changed, 9 insertions(+), 3 deletions(-) diff --git a/selfdrive/ui/qt/widgets/cameraview.cc b/selfdrive/ui/qt/widgets/cameraview.cc index 998e01cbf93e68..b8c00bd8e43416 100644 --- a/selfdrive/ui/qt/widgets/cameraview.cc +++ b/selfdrive/ui/qt/widgets/cameraview.cc @@ -199,9 +199,15 @@ void CameraViewWidget::updateFrameMat() { const vec3 inf = {{1000., 0., 0.}}; const vec3 Ep = matvecmul3(calibration, inf); const vec3 Kep = matvecmul3(intrinsic_matrix, Ep); - x_offset = (Kep.v[0] / Kep.v[2] - intrinsic_matrix.v[2]) * zoom; - y_offset = (Kep.v[1] / Kep.v[2] - intrinsic_matrix.v[5]) * zoom; - qWarning() << x_offset << y_offset; + + float x_offset_ = (Kep.v[0] / Kep.v[2] - intrinsic_matrix.v[2]) * zoom; + float y_offset_ = (Kep.v[1] / Kep.v[2] - intrinsic_matrix.v[5]) * zoom; + + float max_x_offset = intrinsic_matrix.v[2] * zoom - w / 2 - 5; + float max_y_offset = intrinsic_matrix.v[5] * zoom - h / 2 - 5; + + x_offset = std::clamp(x_offset_, -max_x_offset, max_x_offset); + y_offset = std::clamp(y_offset_, -max_y_offset, max_y_offset); float zx = zoom * 2 * intrinsic_matrix.v[2] / width(); float zy = zoom * 2 * intrinsic_matrix.v[5] / height(); From d78af9f488eb3acdc71129814f0bd61c2d4c1f27 Mon Sep 17 00:00:00 2001 From: Willem Melching Date: Fri, 24 Jun 2022 15:48:45 +0200 Subject: [PATCH 4/7] only use when valid --- selfdrive/ui/qt/onroad.cc | 7 ++++++- selfdrive/ui/qt/widgets/cameraview.h | 6 +----- selfdrive/ui/ui.cc | 1 + selfdrive/ui/ui.h | 8 +++----- 4 files changed, 11 insertions(+), 11 deletions(-) diff --git a/selfdrive/ui/qt/onroad.cc b/selfdrive/ui/qt/onroad.cc index d0f2ce7d9ee4ba..cfd128218dab03 100644 --- a/selfdrive/ui/qt/onroad.cc +++ b/selfdrive/ui/qt/onroad.cc @@ -208,7 +208,12 @@ void NvgWindow::updateState(const UIState &s) { setProperty("engageable", cs.getEngageable() || cs.getEnabled()); setProperty("dmActive", sm["driverMonitoringState"].getDriverMonitoringState().getIsActiveMode()); } - CameraViewWidget::updateCalibration(s.scene.view_from_calib); + + if (s.scene.calibration_valid) { + CameraViewWidget::updateCalibration(s.scene.view_from_calib); + } else { + CameraViewWidget::updateCalibration(DEFAULT_CALIBRATION); + } } void NvgWindow::drawHud(QPainter &p) { diff --git a/selfdrive/ui/qt/widgets/cameraview.h b/selfdrive/ui/qt/widgets/cameraview.h index 3f2a5d20bddaf0..6e51e6e38a3385 100644 --- a/selfdrive/ui/qt/widgets/cameraview.h +++ b/selfdrive/ui/qt/widgets/cameraview.h @@ -68,11 +68,7 @@ class CameraViewWidget : public QOpenGLWidget, protected QOpenGLFunctions { int stream_stride = 0; float x_offset = 0; float y_offset = 0; - mat3 calibration = {{ - 0.0, 1.0, 0.0, - 0.0, 0.0, 1.0, - 1.0, 0.0, 0.0 - }}; + mat3 calibration = DEFAULT_CALIBRATION; std::atomic stream_type; QThread *vipc_thread = nullptr; diff --git a/selfdrive/ui/ui.cc b/selfdrive/ui/ui.cc index 4d1e1ab7462593..e844bf72a4f963 100644 --- a/selfdrive/ui/ui.cc +++ b/selfdrive/ui/ui.cc @@ -140,6 +140,7 @@ static void update_state(UIState *s) { scene.view_from_calib.v[i*3 + j] = view_from_calib(i,j); } } + scene.calibration_valid = sm["liveCalibration"].getLiveCalibration().getCalStatus() != 2; } if (s->worldObjectsVisible()) { if (sm.updated("modelV2")) { diff --git a/selfdrive/ui/ui.h b/selfdrive/ui/ui.h index b4c86fab10822e..08b92ebe485a7a 100644 --- a/selfdrive/ui/ui.h +++ b/selfdrive/ui/ui.h @@ -24,6 +24,7 @@ typedef cereal::CarControl::HUDControl::AudibleAlert AudibleAlert; // TODO: choose based on frame input size const float ZOOM = 2912.8; +const mat3 DEFAULT_CALIBRATION = {{ 0.0, 1.0, 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0 }}; struct Alert { QString text1; @@ -91,11 +92,8 @@ typedef struct { } line_vertices_data; typedef struct UIScene { - mat3 view_from_calib = {{ - 0.0, 1.0, 0.0, - 0.0, 0.0, 1.0, - 1.0, 0.0, 0.0 - }}; + bool calibration_valid = false; + mat3 view_from_calib = DEFAULT_CALIBRATION; cereal::PandaState::PandaType pandaType; // modelV2 From cd02d61e0dda8084ab686dc1e76b6eb8e891dbf5 Mon Sep 17 00:00:00 2001 From: Willem Melching Date: Fri, 24 Jun 2022 16:02:33 +0200 Subject: [PATCH 5/7] not while calibrating --- selfdrive/ui/ui.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/selfdrive/ui/ui.cc b/selfdrive/ui/ui.cc index e844bf72a4f963..c8fc645cf2031d 100644 --- a/selfdrive/ui/ui.cc +++ b/selfdrive/ui/ui.cc @@ -140,7 +140,7 @@ static void update_state(UIState *s) { scene.view_from_calib.v[i*3 + j] = view_from_calib(i,j); } } - scene.calibration_valid = sm["liveCalibration"].getLiveCalibration().getCalStatus() != 2; + scene.calibration_valid = sm["liveCalibration"].getLiveCalibration().getCalStatus() == 1; } if (s->worldObjectsVisible()) { if (sm.updated("modelV2")) { From 59cf95d2952cd7c75f98e617c5918c9bc6ac5e86 Mon Sep 17 00:00:00 2001 From: Willem Melching Date: Fri, 24 Jun 2022 16:18:47 +0200 Subject: [PATCH 6/7] less diff --- selfdrive/ui/qt/onroad.cc | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/selfdrive/ui/qt/onroad.cc b/selfdrive/ui/qt/onroad.cc index cfd128218dab03..9a9d152676265e 100644 --- a/selfdrive/ui/qt/onroad.cc +++ b/selfdrive/ui/qt/onroad.cc @@ -406,8 +406,8 @@ void NvgWindow::initializeGL() { } void NvgWindow::updateFrameMat() { - UIState *s = uiState(); CameraViewWidget::updateFrameMat(); + UIState *s = uiState(); int w = width(), h = height(); s->fb_w = w; @@ -421,7 +421,6 @@ void NvgWindow::updateFrameMat() { // 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) From 8cac9d6af53cb5cea94c370f46034ee1b4a866d7 Mon Sep 17 00:00:00 2001 From: Willem Melching Date: Mon, 27 Jun 2022 14:43:35 +0200 Subject: [PATCH 7/7] cleanup zoom --- selfdrive/ui/qt/onroad.cc | 6 +----- selfdrive/ui/qt/widgets/cameraview.cc | 7 ++----- selfdrive/ui/qt/widgets/cameraview.h | 8 ++++++-- selfdrive/ui/ui.h | 2 -- 4 files changed, 9 insertions(+), 14 deletions(-) diff --git a/selfdrive/ui/qt/onroad.cc b/selfdrive/ui/qt/onroad.cc index 9a9d152676265e..ecd6d61471cf8b 100644 --- a/selfdrive/ui/qt/onroad.cc +++ b/selfdrive/ui/qt/onroad.cc @@ -412,11 +412,7 @@ void NvgWindow::updateFrameMat() { s->fb_w = w; s->fb_h = h; - auto intrinsic_matrix = s->wide_camera ? ecam_intrinsic_matrix : fcam_intrinsic_matrix; - float zoom = ZOOM / intrinsic_matrix.v[0]; - if (s->wide_camera) { - zoom *= 0.5; - } + // 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 diff --git a/selfdrive/ui/qt/widgets/cameraview.cc b/selfdrive/ui/qt/widgets/cameraview.cc index b8c00bd8e43416..a80672f2c36992 100644 --- a/selfdrive/ui/qt/widgets/cameraview.cc +++ b/selfdrive/ui/qt/widgets/cameraview.cc @@ -187,11 +187,8 @@ void CameraViewWidget::updateFrameMat() { if (stream_type == VISION_STREAM_DRIVER) { frame_mat = get_driver_view_transform(w, h, stream_width, stream_height); } else { - auto intrinsic_matrix = stream_type == VISION_STREAM_WIDE_ROAD ? ecam_intrinsic_matrix : fcam_intrinsic_matrix; - float zoom = ZOOM / intrinsic_matrix.v[0]; - if (stream_type == VISION_STREAM_WIDE_ROAD) { - zoom *= 0.5; - } + intrinsic_matrix = (stream_type == VISION_STREAM_WIDE_ROAD) ? ecam_intrinsic_matrix : fcam_intrinsic_matrix; + zoom = (stream_type == VISION_STREAM_WIDE_ROAD) ? 2.5 : 1.1; // Project point at "infinity" to compute x and y offsets // to ensure this ends up in the middle of the screen diff --git a/selfdrive/ui/qt/widgets/cameraview.h b/selfdrive/ui/qt/widgets/cameraview.h index 6e51e6e38a3385..cc11ec2c277d0e 100644 --- a/selfdrive/ui/qt/widgets/cameraview.h +++ b/selfdrive/ui/qt/widgets/cameraview.h @@ -66,11 +66,15 @@ class CameraViewWidget : public QOpenGLWidget, protected QOpenGLFunctions { int stream_width = 0; int stream_height = 0; int stream_stride = 0; + std::atomic stream_type; + QThread *vipc_thread = nullptr; + + // Calibration float x_offset = 0; float y_offset = 0; + float zoom = 1.0; mat3 calibration = DEFAULT_CALIBRATION; - std::atomic stream_type; - QThread *vipc_thread = nullptr; + mat3 intrinsic_matrix = fcam_intrinsic_matrix; std::deque> frames; uint32_t draw_frame_id = 0; diff --git a/selfdrive/ui/ui.h b/selfdrive/ui/ui.h index 08b92ebe485a7a..7364b81a40833b 100644 --- a/selfdrive/ui/ui.h +++ b/selfdrive/ui/ui.h @@ -22,8 +22,6 @@ const int footer_h = 280; const int UI_FREQ = 20; // Hz typedef cereal::CarControl::HUDControl::AudibleAlert AudibleAlert; -// TODO: choose based on frame input size -const float ZOOM = 2912.8; const mat3 DEFAULT_CALIBRATION = {{ 0.0, 1.0, 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0 }}; struct Alert {