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

UI: fade into wide camera (#26203)

Browse Source 
* UI: fade into wide camera

* handle routes with no wide calib

* more cleanup

Co-authored-by: Adeeb Shihadeh <adeebshihadeh@gmail.com>
This commit is contained in:
HaraldSchafer
2022-10-27 17:27:31 -07:00
committed by GitHub
parent 26cb6d09e7
commit dbc30c053c
5 changed files with 37 additions and 20 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/locationd/calibrationd.py
+1 -6
View File
@@ -66,7 +66,6 @@ class Calibrator:
# Read saved calibration
params = Params()
calibration_params = params.get("CalibrationParams")
self.wide_camera = params.get_bool('WideCameraOnly')
rpy_init = RPY_INIT
wide_from_device_euler = WIDE_FROM_DEVICE_EULER_INIT
valid_blocks = 0
@@ -166,10 +165,7 @@ class Calibrator:
self.old_rpy_weight = min(0.0, self.old_rpy_weight - 1/SMOOTH_CYCLES)
straight_and_fast = ((self.v_ego > MIN_SPEED_FILTER) and (trans[0] > MIN_SPEED_FILTER) and (abs(rot[2]) < MAX_YAW_RATE_FILTER))
if self.wide_camera:
angle_std_threshold = 4*MAX_VEL_ANGLE_STD
else:
angle_std_threshold = MAX_VEL_ANGLE_STD
angle_std_threshold = MAX_VEL_ANGLE_STD
certain_if_calib = ((np.arctan2(trans_std[1], trans[0]) < angle_std_threshold) or
(self.valid_blocks < INPUTS_NEEDED))
if not (straight_and_fast and certain_if_calib):
@@ -185,7 +181,6 @@ class Calibrator:
new_wide_from_device_euler = np.array(wide_from_device_euler)
else:
new_wide_from_device_euler = WIDE_FROM_DEVICE_EULER_INIT
self.rpys[self.block_idx] = (self.idx*self.rpys[self.block_idx] +
(BLOCK_SIZE - self.idx) * new_rpy) / float(BLOCK_SIZE)
self.wide_from_device_eulers[self.block_idx] = (self.idx*self.wide_from_device_eulers[self.block_idx] +
selfdrive/ui/qt/onroad.cc
+3 -5
View File
@@ -100,10 +100,6 @@ void OnroadWindow::offroadTransition(bool offroad) {
#endif
alerts->updateAlert({}, bg);
// update stream type
bool wide_cam = Params().getBool("WideCameraOnly");
nvg->setStreamType(wide_cam ? VISION_STREAM_WIDE_ROAD : VISION_STREAM_ROAD);
}
void OnroadWindow::paintEvent(QPaintEvent *event) {
@@ -232,8 +228,10 @@ void AnnotatedCameraWidget::updateState(const UIState &s) {
setProperty("rightHandDM", sm["driverMonitoringState"].getDriverMonitoringState().getIsRHD());
}
setStreamType(s.scene.wide_cam ? VISION_STREAM_WIDE_ROAD : VISION_STREAM_ROAD);
if (s.scene.calibration_valid) {
CameraWidget::updateCalibration(s.scene.view_from_calib);
auto calib = s.scene.wide_cam ? s.scene.view_from_wide_calib : s.scene.view_from_calib;
CameraWidget::updateCalibration(calib);
} else {
CameraWidget::updateCalibration(DEFAULT_CALIBRATION);
}
selfdrive/ui/qt/widgets/cameraview.cc
+8 -3
View File
@@ -188,12 +188,17 @@ void CameraWidget::updateFrameMat() {
if (stream_type == VISION_STREAM_DRIVER) {
frame_mat = get_driver_view_transform(w, h, stream_width, stream_height);
} else {
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
// for narrow come and a little lower for wide cam.
// TODO: use proper perspective transform?
if (stream_type == VISION_STREAM_WIDE_ROAD) {
intrinsic_matrix = ecam_intrinsic_matrix;
zoom = 2.0;
} else {
intrinsic_matrix = fcam_intrinsic_matrix;
zoom = 1.1;
}
const vec3 inf = {{1000., 0., 0.}};
const vec3 Ep = matvecmul3(calibration, inf);
const vec3 Kep = matvecmul3(intrinsic_matrix, Ep);
selfdrive/ui/ui.cc
+22 -5
View File
@@ -23,8 +23,8 @@ static bool calib_frame_to_full_frame(const UIState *s, float in_x, float in_y,
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.view_from_calib, pt);
const vec3 KEp = matvecmul3(s->wide_camera ? ecam_intrinsic_matrix : fcam_intrinsic_matrix, Ep);
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]});
@@ -121,17 +121,23 @@ static void update_state(UIState *s) {
if (sm.updated("liveCalibration")) {
auto rpy_list = sm["liveCalibration"].getLiveCalibration().getRpyCalib();
auto wfde_list = sm["liveCalibration"].getLiveCalibration().getWideFromDeviceEuler();
Eigen::Vector3d rpy;
rpy << rpy_list[0], rpy_list[1], rpy_list[2];
Eigen::Vector3d wfde;
if (rpy_list.size() == 3) rpy << rpy_list[0], rpy_list[1], rpy_list[2];
if (wfde_list.size() == 3) wfde << wfde_list[0], wfde_list[1], wfde_list[2];
Eigen::Matrix3d device_from_calib = euler2rot(rpy);
Eigen::Matrix3d wide_from_device = euler2rot(wfde);
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;
Eigen::Matrix3d view_from_wide_calib = view_from_device * wide_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);
scene.view_from_wide_calib.v[i*3 + j] = view_from_wide_calib(i,j);
}
}
scene.calibration_valid = sm["liveCalibration"].getLiveCalibration().getCalStatus() == 1;
@@ -167,6 +173,17 @@ static void update_state(UIState *s) {
scene.light_sensor = std::max(100.0f - scale * sm["wideRoadCameraState"].getWideRoadCameraState().getExposureValPercent(), 0.0f);
}
scene.started = sm["deviceState"].getDeviceState().getStarted() && scene.ignition;
if (sm.updated("carState")) {
float v_ego = sm["carState"].getCarState().getVEgo();
// TODO: support replays without ecam by using fcam
// Wide or narrow cam dependent on speed
if ((v_ego < 10) || s->wide_cam_only) {
scene.wide_cam = true;
} else if (v_ego > 15) {
scene.wide_cam = false;
}
}
}
void ui_update_params(UIState *s) {
@@ -197,7 +214,7 @@ void UIState::updateStatus() {
if (scene.started) {
status = STATUS_DISENGAGED;
scene.started_frame = sm->frame;
wide_camera = Params().getBool("WideCameraOnly");
wide_cam_only = Params().getBool("WideCameraOnly");
}
started_prev = scene.started;
emit offroadTransition(!scene.started);
@@ -219,7 +236,7 @@ UIState::UIState(QObject *parent) : QObject(parent) {
});
Params params;
wide_camera = params.getBool("WideCameraOnly");
wide_cam_only = params.getBool("WideCameraOnly");
prime_type = std::atoi(params.get("PrimeType").c_str());
language = QString::fromStdString(params.get("LanguageSetting"));
selfdrive/ui/ui.h
+3 -1
View File
@@ -87,7 +87,9 @@ const QColor bg_colors [] = {
typedef struct UIScene {
bool calibration_valid = false;
bool wide_cam = true;
mat3 view_from_calib = DEFAULT_CALIBRATION;
mat3 view_from_wide_calib = DEFAULT_CALIBRATION;
cereal::PandaState::PandaType pandaType;
// modelV2
@@ -130,7 +132,7 @@ public:
QString language;
QTransform car_space_transform;
bool wide_camera;
bool wide_cam_only;
signals:
void uiUpdate(const UIState &s);