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>pull/20567/head
Willem Melching
GitHub
parent
ae094042ad
commit
1fc7bcca43
|
|
@ -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],
|
||||
|
|
|
|||
|
|
@ -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) {
|
||||
|
|
|
|||
|
|
@ -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 = []
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
|
|
@ -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);
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
|
|
|
|||
|
|
@ -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; }
|
||||
));
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
|
|
|
|||
Loading…
Reference in New Issue