mawei/dragonpilot
1
0
Fork 0
mirror of https://github.com/dragonpilot/dragonpilot.git synced 2026-06-21 22:12:05 +08:00
Code Issues Packages Projects Releases Wiki Activity

ui: use current calibration to center vanishing point (#24955)

Browse Source 
* compute x and y offsets using calibration

* fix default calibration

* clamp to max values

* only use when valid

* not while calibrating

* less diff

* cleanup zoom
This commit is contained in:
Willem Melching
2022-06-27 16:03:26 +02:00
committed by GitHub
parent 240c44e50c
commit 915b4928ff
6 changed files with 62 additions and 37 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/ui/qt/onroad.cc
+12 -9
View File
@@ -208,6 +208,12 @@ void NvgWindow::updateState(const UIState &s) {
setProperty("engageable", cs.getEngageable() || cs.getEnabled());
setProperty("dmActive", sm["driverMonitoringState"].getDriverMonitoringState().getIsActiveMode());
}
if (s.scene.calibration_valid) {
CameraViewWidget::updateCalibration(s.scene.view_from_calib);
} else {
CameraViewWidget::updateCalibration(DEFAULT_CALIBRATION);
}
}
void NvgWindow::drawHud(QPainter &p) {
@@ -399,23 +405,20 @@ void NvgWindow::initializeGL() {
setBackgroundColor(bg_colors[STATUS_DISENGAGED]);
}
void NvgWindow::updateFrameMat(int w, int h) {
CameraViewWidget::updateFrameMat(w, h);
void NvgWindow::updateFrameMat() {
CameraViewWidget::updateFrameMat();
UIState *s = uiState();
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;
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
// 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]);
}
selfdrive/ui/qt/onroad.h
+1 -1
View File
@@ -70,7 +70,7 @@ protected:
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);
selfdrive/ui/qt/widgets/cameraview.cc
+35 -20
View File
@@ -6,6 +6,8 @@
#include <GLES3/gl3.h>
#endif
#include <cmath>
#include <QOpenGLBuffer>
#include <QOffscreenSurface>
@@ -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,55 @@ 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;
}
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
// TODO: use proper perspective transform?
const vec3 inf = {{1000., 0., 0.}};
const vec3 Ep = matvecmul3(calibration, inf);
const vec3 Kep = matvecmul3(intrinsic_matrix, Ep);
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();
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 +326,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) {
selfdrive/ui/qt/widgets/cameraview.h
+10 -2
View File
@@ -42,11 +42,12 @@ signals:
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;
@@ -68,6 +69,13 @@ protected:
std::atomic<VisionStreamType> stream_type;
QThread *vipc_thread = nullptr;
// Calibration
float x_offset = 0;
float y_offset = 0;
float zoom = 1.0;
mat3 calibration = DEFAULT_CALIBRATION;
mat3 intrinsic_matrix = fcam_intrinsic_matrix;
std::deque<std::pair<uint32_t, VisionBuf*>> frames;
uint32_t draw_frame_id = 0;
selfdrive/ui/ui.cc
+1
View File
@@ -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() == 1;
}
if (s->worldObjectsVisible()) {
if (sm.updated("modelV2")) {
selfdrive/ui/ui.h
+3 -5
View File
@@ -22,10 +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;
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;
@@ -93,7 +90,8 @@ typedef struct {
} line_vertices_data;
typedef struct UIScene {
mat3 view_from_calib;
bool calibration_valid = false;
mat3 view_from_calib = DEFAULT_CALIBRATION;
cereal::PandaState::PandaType pandaType;
// modelV2