diff --git a/common/params_pyx.pyx b/common/params_pyx.pyx index a062de0b7a..475931cacb 100755 --- a/common/params_pyx.pyx +++ b/common/params_pyx.pyx @@ -29,6 +29,7 @@ keys = { b"CompletedTrainingVersion": [TxType.PERSISTENT], b"DisablePowerDown": [TxType.PERSISTENT], b"DisableUpdates": [TxType.PERSISTENT], + b"EnableWideCamera": [TxType.PERSISTENT], b"DoUninstall": [TxType.CLEAR_ON_MANAGER_START], b"DongleId": [TxType.PERSISTENT], b"GitDiff": [TxType.PERSISTENT], diff --git a/selfdrive/common/modeldata.h b/selfdrive/common/modeldata.h index fbbd9ce46c..7604df65e2 100644 --- a/selfdrive/common/modeldata.h +++ b/selfdrive/common/modeldata.h @@ -26,12 +26,25 @@ const mat3 fcam_intrinsic_matrix = (mat3){{ 0.0, 2648.0, 1208.0/2, 0.0, 0.0, 1.0 }}; + +// without unwarp, focal length is for center portion only +const mat3 ecam_intrinsic_matrix = (mat3){{ + 620.0, 0.0, 1928.0/2, + 0.0, 620.0, 1208.0/2, + 0.0, 0.0, 1.0 +}}; #else const mat3 fcam_intrinsic_matrix = (mat3){{ 910., 0., 1164.0/2, 0., 910., 874.0/2, 0., 0., 1. }}; + +const mat3 ecam_intrinsic_matrix = (mat3){{ + 0., 0., 0., + 0., 0., 0., + 0., 0., 0. +}}; #endif static inline mat3 get_model_yuv_transform(bool bayer = true) { diff --git a/selfdrive/controls/lib/lane_planner.py b/selfdrive/controls/lib/lane_planner.py index 200ab5d3a3..79f23294ba 100644 --- a/selfdrive/controls/lib/lane_planner.py +++ b/selfdrive/controls/lib/lane_planner.py @@ -17,9 +17,8 @@ else: PATH_OFFSET = 0.0 - class LanePlanner: - def __init__(self): + def __init__(self, wide_camera=False): self.ll_t = np.zeros((TRAJECTORY_SIZE,)) self.ll_x = np.zeros((TRAJECTORY_SIZE,)) self.lll_y = np.zeros((TRAJECTORY_SIZE,)) @@ -38,6 +37,8 @@ class LanePlanner: self.l_lane_change_prob = 0. self.r_lane_change_prob = 0. + self.camera_offset = -CAMERA_OFFSET if wide_camera else CAMERA_OFFSET + self.path_offset = -PATH_OFFSET if wide_camera else PATH_OFFSET def parse_model(self, md): if len(md.laneLines) == 4 and len(md.laneLines[0].t) == TRAJECTORY_SIZE: @@ -45,8 +46,8 @@ class LanePlanner: # left and right ll x is the same self.ll_x = md.laneLines[1].x # only offset left and right lane lines; offsetting path does not make sense - self.lll_y = np.array(md.laneLines[1].y) - CAMERA_OFFSET - self.rll_y = np.array(md.laneLines[2].y) - CAMERA_OFFSET + self.lll_y = np.array(md.laneLines[1].y) - self.camera_offset + self.rll_y = np.array(md.laneLines[2].y) - self.camera_offset self.lll_prob = md.laneLineProbs[1] self.rll_prob = md.laneLineProbs[2] self.lll_std = md.laneLineStds[1] @@ -59,7 +60,7 @@ class LanePlanner: def get_d_path(self, v_ego, path_t, path_xyz): # Reduce reliance on lanelines that are too far apart or # will be in a few seconds - path_xyz[:,1] -= PATH_OFFSET + path_xyz[:, 1] -= self.path_offset l_prob, r_prob = self.lll_prob, self.rll_prob width_pts = self.rll_y - self.lll_y prob_mods = [] diff --git a/selfdrive/controls/lib/lateral_planner.py b/selfdrive/controls/lib/lateral_planner.py index 693d184a4f..394175f44b 100644 --- a/selfdrive/controls/lib/lateral_planner.py +++ b/selfdrive/controls/lib/lateral_planner.py @@ -9,6 +9,7 @@ from selfdrive.controls.lib.lateral_mpc import libmpc_py from selfdrive.controls.lib.drive_helpers import MPC_COST_LAT, MPC_N, CAR_ROTATION_RADIUS from selfdrive.controls.lib.lane_planner import LanePlanner, TRAJECTORY_SIZE from selfdrive.config import Conversions as CV +from selfdrive.hardware import TICI import cereal.messaging as messaging from cereal import log @@ -47,14 +48,17 @@ DESIRES = { class LateralPlanner(): def __init__(self, CP): - self.LP = LanePlanner() + params = Params() + + wide_camera = (params.get('EnableWideCamera') == b'1') if TICI else False + self.LP = LanePlanner(wide_camera) self.last_cloudlog_t = 0 self.steer_rate_cost = CP.steerRateCost self.setup_mpc() self.solution_invalid_cnt = 0 - self.use_lanelines = not Params().get_bool('EndToEndToggle') + self.use_lanelines = not params.get_bool('EndToEndToggle') self.lane_change_state = LaneChangeState.off self.lane_change_direction = LaneChangeDirection.none self.lane_change_timer = 0.0 diff --git a/selfdrive/modeld/modeld.cc b/selfdrive/modeld/modeld.cc index e967d5a754..03a9dc758f 100644 --- a/selfdrive/modeld/modeld.cc +++ b/selfdrive/modeld/modeld.cc @@ -7,6 +7,7 @@ #include "common/swaglog.h" #include "common/clutil.h" #include "common/util.h" +#include "common/params.h" #include "models/driving.h" #include "messaging.hpp" @@ -17,7 +18,7 @@ bool live_calib_seen; mat3 cur_transform; std::mutex transform_lock; -void calibration_thread() { +void calibration_thread(bool wide_camera) { set_thread_name("calibration"); set_realtime_priority(50); @@ -35,7 +36,7 @@ void calibration_thread() { -1.09890110e-03, 0.00000000e+00, 2.81318681e-01, -1.84808520e-20, 9.00738606e-04,-4.28751576e-02; - Eigen::Matrix fcam_intrinsics = Eigen::Matrix(fcam_intrinsic_matrix.v); + Eigen::Matrix cam_intrinsics = Eigen::Matrix(wide_camera ? ecam_intrinsic_matrix.v : fcam_intrinsic_matrix.v); const mat3 yuv_transform = get_model_yuv_transform(); while (!do_exit) { @@ -47,7 +48,7 @@ void calibration_thread() { extrinsic_matrix_eigen(i / 4, i % 4) = extrinsic_matrix[i]; } - auto camera_frame_from_road_frame = fcam_intrinsics * extrinsic_matrix_eigen; + auto camera_frame_from_road_frame = cam_intrinsics * extrinsic_matrix_eigen; Eigen::Matrix camera_frame_from_ground; camera_frame_from_ground.col(0) = camera_frame_from_road_frame.col(0); camera_frame_from_ground.col(1) = camera_frame_from_road_frame.col(1); @@ -116,7 +117,7 @@ void run_model(ModelState &model, VisionIpcClient &vipc_client) { frame_dropped_filter.reset(0); frames_dropped = 0.; } - + float frame_drop_ratio = frames_dropped / (1 + frames_dropped); model_publish(pm, extra.frame_id, frame_id, frame_drop_ratio, model_buf, extra.timestamp_eof, model_execution_time, @@ -140,8 +141,14 @@ int main(int argc, char **argv) { set_core_affinity(4); #endif + bool wide_camera = false; + +#ifdef QCOM2 + wide_camera = Params().getBool("EnableWideCamera"); +#endif + // start calibration thread - std::thread thread = std::thread(calibration_thread); + std::thread thread = std::thread(calibration_thread, wide_camera); // cl init cl_device_id device_id = cl_get_device_id(CL_DEVICE_TYPE_DEFAULT); @@ -152,7 +159,7 @@ int main(int argc, char **argv) { model_init(&model, device_id, context); LOGW("models loaded, modeld starting"); - VisionIpcClient vipc_client = VisionIpcClient("camerad", VISION_STREAM_YUV_BACK, true, device_id, context); + VisionIpcClient vipc_client = VisionIpcClient("camerad", wide_camera ? VISION_STREAM_YUV_WIDE : VISION_STREAM_YUV_BACK, true, device_id, context); while (!do_exit && !vipc_client.connect(false)) { util::sleep_for(100); } diff --git a/selfdrive/ui/paint.cc b/selfdrive/ui/paint.cc index 771a29d3f5..132c52cb24 100644 --- a/selfdrive/ui/paint.cc +++ b/selfdrive/ui/paint.cc @@ -15,10 +15,10 @@ // TODO: choose based on frame input size #ifdef QCOM2 const float y_offset = 150.0; -const float zoom = 1.1; +const float zoom = 2912.8; #else const float y_offset = 0.0; -const float zoom = 2.35; +const float zoom = 2138.5; #endif static void ui_draw_text(const UIState *s, float x, float y, const char *string, float size, NVGcolor color, const char *font_name) { @@ -83,7 +83,7 @@ static void draw_lead(UIState *s, int idx) { fillAlpha = (int)(fmin(fillAlpha, 255)); } - float sz = std::clamp((25 * 30) / (d_rel / 3 + 30), 15.0f, 30.0f) * zoom; + float sz = std::clamp((25 * 30) / (d_rel / 3 + 30), 15.0f, 30.0f) * s->zoom; x = std::clamp(x, 0.f, s->viz_rect.right() - sz / 2); y = std::fmin(s->viz_rect.bottom() - sz * .6, y); draw_chevron(s, x, y, sz, nvgRGBA(201, 34, 49, fillAlpha), COLOR_YELLOW); @@ -312,7 +312,7 @@ static void ui_draw_vision_alert(UIState *s) { .h = alr_h}; ui_fill_rect(s->vg, rect, color); - ui_fill_rect(s->vg, rect, nvgLinearGradient(s->vg, rect.x, rect.y, rect.x, rect.bottom(), + ui_fill_rect(s->vg, rect, nvgLinearGradient(s->vg, rect.x, rect.y, rect.x, rect.bottom(), nvgRGBAf(0.0, 0.0, 0.0, 0.05), nvgRGBAf(0.0, 0.0, 0.0, 0.35))); nvgFillColor(s->vg, COLOR_WHITE); @@ -593,9 +593,17 @@ void ui_nvg_init(UIState *s) { glBindVertexArray(0); } + auto intrinsic_matrix = s->wide_camera ? ecam_intrinsic_matrix : fcam_intrinsic_matrix; + + s->zoom = zoom / intrinsic_matrix.v[0]; + + if (s->wide_camera) { + s->zoom *= 0.5; + } + s->video_rect = Rect{bdr_s, bdr_s, s->fb_w - 2 * bdr_s, s->fb_h - 2 * bdr_s}; - float zx = zoom * 2 * fcam_intrinsic_matrix.v[2] / s->video_rect.w; - float zy = zoom * 2 * fcam_intrinsic_matrix.v[5] / s->video_rect.h; + float zx = s->zoom * 2 * intrinsic_matrix.v[2] / s->video_rect.w; + float zy = s->zoom * 2 * intrinsic_matrix.v[5] / s->video_rect.h; const mat4 frame_transform = {{ zx, 0.0, 0.0, 0.0, @@ -612,10 +620,10 @@ void ui_nvg_init(UIState *s) { nvgTranslate(s->vg, s->video_rect.x + s->video_rect.w / 2, s->video_rect.y + s->video_rect.h / 2 + y_offset); // 2) Apply same scaling as video - nvgScale(s->vg, zoom, zoom); + nvgScale(s->vg, s->zoom, s->zoom); // 3) Put (0, 0) in top left corner of video - nvgTranslate(s->vg, -fcam_intrinsic_matrix.v[2], -fcam_intrinsic_matrix.v[5]); + nvgTranslate(s->vg, -intrinsic_matrix.v[2], -intrinsic_matrix.v[5]); nvgCurrentTransform(s->vg, s->car_space_transform); nvgResetTransform(s->vg); diff --git a/selfdrive/ui/qt/offroad/settings.cc b/selfdrive/ui/qt/offroad/settings.cc index 624cbc8f5f..8c59e75807 100644 --- a/selfdrive/ui/qt/offroad/settings.cc +++ b/selfdrive/ui/qt/offroad/settings.cc @@ -63,6 +63,14 @@ QWidget * toggles_panel() { "In this mode openpilot will ignore lanelines and just drive how it thinks a human would.", "../assets/offroad/icon_road.png")); +#ifdef QCOM2 + toggles_list->addWidget(horizontal_line()); + toggles_list->addWidget(new ParamControl("EnableWideCamera", + "Enable use of Wide Angle Camera", + "Use wide angle camera for driving and ui. Only takes effect after reboot.", + "../assets/offroad/icon_openpilot.png")); +#endif + bool record_lock = Params().getBool("RecordFrontLock"); record_toggle->setEnabled(!record_lock); @@ -88,7 +96,7 @@ DevicePanel::DevicePanel(QWidget* parent) : QWidget(parent) { offroad_btns.append(new ButtonControl("Driver Camera", "PREVIEW", "Preview the driver facing camera to help optimize device mounting position for best driver monitoring experience. (vehicle must be off)", - [=]() { + [=]() { Params().putBool("IsDriverViewEnabled", true); GLWindow::ui_state.scene.driver_view = true; } )); diff --git a/selfdrive/ui/ui.cc b/selfdrive/ui/ui.cc index 96c1dcbfc4..4820765c9c 100644 --- a/selfdrive/ui/ui.cc +++ b/selfdrive/ui/ui.cc @@ -16,7 +16,7 @@ static bool calib_frame_to_full_frame(const UIState *s, float in_x, float in_y, const float margin = 500.0f; const vec3 pt = (vec3){{in_x, in_y, in_z}}; const vec3 Ep = matvecmul3(s->scene.view_from_calib, pt); - const vec3 KEp = matvecmul3(fcam_intrinsic_matrix, Ep); + const vec3 KEp = matvecmul3(s->wide_camera ? ecam_intrinsic_matrix : fcam_intrinsic_matrix, Ep); // Project. float x = KEp.v[0] / KEp.v[2]; @@ -58,10 +58,17 @@ void ui_init(UIState *s) { s->scene.started = false; s->status = STATUS_OFFROAD; - ui_nvg_init(s); s->last_frame = nullptr; - s->vipc_client_rear = new VisionIpcClient("camerad", VISION_STREAM_RGB_BACK, true); + s->wide_camera = false; + +#ifdef QCOM2 + s->wide_camera = Params().getBool("EnableWideCamera"); +#endif + + ui_nvg_init(s); + + s->vipc_client_rear = new VisionIpcClient("camerad", s->wide_camera ? VISION_STREAM_RGB_WIDE : VISION_STREAM_RGB_BACK, true); s->vipc_client_front = new VisionIpcClient("camerad", VISION_STREAM_RGB_FRONT, true); s->vipc_client = s->vipc_client_rear; } diff --git a/selfdrive/ui/ui.hpp b/selfdrive/ui/ui.hpp index 2f0134eb9c..91b1820833 100644 --- a/selfdrive/ui/ui.hpp +++ b/selfdrive/ui/ui.hpp @@ -167,6 +167,8 @@ typedef struct UIState { bool sidebar_collapsed; Rect video_rect, viz_rect; float car_space_transform[6]; + bool wide_camera; + float zoom; } UIState; void ui_init(UIState *s);