ecam toggle (#20597)

* use ecam in ui

* needs reboot

* use in modeld

* typo

* effective FL is longer in center

* normalize zoom by focal dist

* read param before init

* make 2x as wide

* review comments

* more explicit

* fix camera offset

Co-authored-by: ZwX1616 <zwx1616@gmail.com>

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],

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) {

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 = []

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

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<float, 3, 3> fcam_intrinsics = Eigen::Matrix<float, 3, 3, Eigen::RowMajor>(fcam_intrinsic_matrix.v);
+  Eigen::Matrix<float, 3, 3> cam_intrinsics = Eigen::Matrix<float, 3, 3, Eigen::RowMajor>(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<float, 3, 3> 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);
@@ -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);
   }

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);
@@ -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);

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);
 

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;
 }

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);