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
parent
ae094042ad
commit
1fc7bcca43
|
@ -29,6 +29,7 @@ keys = {
|
||||||
b"CompletedTrainingVersion": [TxType.PERSISTENT],
|
b"CompletedTrainingVersion": [TxType.PERSISTENT],
|
||||||
b"DisablePowerDown": [TxType.PERSISTENT],
|
b"DisablePowerDown": [TxType.PERSISTENT],
|
||||||
b"DisableUpdates": [TxType.PERSISTENT],
|
b"DisableUpdates": [TxType.PERSISTENT],
|
||||||
|
b"EnableWideCamera": [TxType.PERSISTENT],
|
||||||
b"DoUninstall": [TxType.CLEAR_ON_MANAGER_START],
|
b"DoUninstall": [TxType.CLEAR_ON_MANAGER_START],
|
||||||
b"DongleId": [TxType.PERSISTENT],
|
b"DongleId": [TxType.PERSISTENT],
|
||||||
b"GitDiff": [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, 2648.0, 1208.0/2,
|
||||||
0.0, 0.0, 1.0
|
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
|
#else
|
||||||
const mat3 fcam_intrinsic_matrix = (mat3){{
|
const mat3 fcam_intrinsic_matrix = (mat3){{
|
||||||
910., 0., 1164.0/2,
|
910., 0., 1164.0/2,
|
||||||
0., 910., 874.0/2,
|
0., 910., 874.0/2,
|
||||||
0., 0., 1.
|
0., 0., 1.
|
||||||
}};
|
}};
|
||||||
|
|
||||||
|
const mat3 ecam_intrinsic_matrix = (mat3){{
|
||||||
|
0., 0., 0.,
|
||||||
|
0., 0., 0.,
|
||||||
|
0., 0., 0.
|
||||||
|
}};
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
static inline mat3 get_model_yuv_transform(bool bayer = true) {
|
static inline mat3 get_model_yuv_transform(bool bayer = true) {
|
||||||
|
|
|
@ -17,9 +17,8 @@ else:
|
||||||
PATH_OFFSET = 0.0
|
PATH_OFFSET = 0.0
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
class LanePlanner:
|
class LanePlanner:
|
||||||
def __init__(self):
|
def __init__(self, wide_camera=False):
|
||||||
self.ll_t = np.zeros((TRAJECTORY_SIZE,))
|
self.ll_t = np.zeros((TRAJECTORY_SIZE,))
|
||||||
self.ll_x = np.zeros((TRAJECTORY_SIZE,))
|
self.ll_x = np.zeros((TRAJECTORY_SIZE,))
|
||||||
self.lll_y = np.zeros((TRAJECTORY_SIZE,))
|
self.lll_y = np.zeros((TRAJECTORY_SIZE,))
|
||||||
|
@ -38,6 +37,8 @@ class LanePlanner:
|
||||||
self.l_lane_change_prob = 0.
|
self.l_lane_change_prob = 0.
|
||||||
self.r_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):
|
def parse_model(self, md):
|
||||||
if len(md.laneLines) == 4 and len(md.laneLines[0].t) == TRAJECTORY_SIZE:
|
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
|
# left and right ll x is the same
|
||||||
self.ll_x = md.laneLines[1].x
|
self.ll_x = md.laneLines[1].x
|
||||||
# only offset left and right lane lines; offsetting path does not make sense
|
# 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.lll_y = np.array(md.laneLines[1].y) - self.camera_offset
|
||||||
self.rll_y = np.array(md.laneLines[2].y) - CAMERA_OFFSET
|
self.rll_y = np.array(md.laneLines[2].y) - self.camera_offset
|
||||||
self.lll_prob = md.laneLineProbs[1]
|
self.lll_prob = md.laneLineProbs[1]
|
||||||
self.rll_prob = md.laneLineProbs[2]
|
self.rll_prob = md.laneLineProbs[2]
|
||||||
self.lll_std = md.laneLineStds[1]
|
self.lll_std = md.laneLineStds[1]
|
||||||
|
@ -59,7 +60,7 @@ class LanePlanner:
|
||||||
def get_d_path(self, v_ego, path_t, path_xyz):
|
def get_d_path(self, v_ego, path_t, path_xyz):
|
||||||
# Reduce reliance on lanelines that are too far apart or
|
# Reduce reliance on lanelines that are too far apart or
|
||||||
# will be in a few seconds
|
# 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
|
l_prob, r_prob = self.lll_prob, self.rll_prob
|
||||||
width_pts = self.rll_y - self.lll_y
|
width_pts = self.rll_y - self.lll_y
|
||||||
prob_mods = []
|
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.drive_helpers import MPC_COST_LAT, MPC_N, CAR_ROTATION_RADIUS
|
||||||
from selfdrive.controls.lib.lane_planner import LanePlanner, TRAJECTORY_SIZE
|
from selfdrive.controls.lib.lane_planner import LanePlanner, TRAJECTORY_SIZE
|
||||||
from selfdrive.config import Conversions as CV
|
from selfdrive.config import Conversions as CV
|
||||||
|
from selfdrive.hardware import TICI
|
||||||
import cereal.messaging as messaging
|
import cereal.messaging as messaging
|
||||||
from cereal import log
|
from cereal import log
|
||||||
|
|
||||||
|
@ -47,14 +48,17 @@ DESIRES = {
|
||||||
|
|
||||||
class LateralPlanner():
|
class LateralPlanner():
|
||||||
def __init__(self, CP):
|
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.last_cloudlog_t = 0
|
||||||
self.steer_rate_cost = CP.steerRateCost
|
self.steer_rate_cost = CP.steerRateCost
|
||||||
|
|
||||||
self.setup_mpc()
|
self.setup_mpc()
|
||||||
self.solution_invalid_cnt = 0
|
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_state = LaneChangeState.off
|
||||||
self.lane_change_direction = LaneChangeDirection.none
|
self.lane_change_direction = LaneChangeDirection.none
|
||||||
self.lane_change_timer = 0.0
|
self.lane_change_timer = 0.0
|
||||||
|
|
|
@ -7,6 +7,7 @@
|
||||||
#include "common/swaglog.h"
|
#include "common/swaglog.h"
|
||||||
#include "common/clutil.h"
|
#include "common/clutil.h"
|
||||||
#include "common/util.h"
|
#include "common/util.h"
|
||||||
|
#include "common/params.h"
|
||||||
|
|
||||||
#include "models/driving.h"
|
#include "models/driving.h"
|
||||||
#include "messaging.hpp"
|
#include "messaging.hpp"
|
||||||
|
@ -17,7 +18,7 @@ bool live_calib_seen;
|
||||||
mat3 cur_transform;
|
mat3 cur_transform;
|
||||||
std::mutex transform_lock;
|
std::mutex transform_lock;
|
||||||
|
|
||||||
void calibration_thread() {
|
void calibration_thread(bool wide_camera) {
|
||||||
set_thread_name("calibration");
|
set_thread_name("calibration");
|
||||||
set_realtime_priority(50);
|
set_realtime_priority(50);
|
||||||
|
|
||||||
|
@ -35,7 +36,7 @@ void calibration_thread() {
|
||||||
-1.09890110e-03, 0.00000000e+00, 2.81318681e-01,
|
-1.09890110e-03, 0.00000000e+00, 2.81318681e-01,
|
||||||
-1.84808520e-20, 9.00738606e-04,-4.28751576e-02;
|
-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();
|
const mat3 yuv_transform = get_model_yuv_transform();
|
||||||
|
|
||||||
while (!do_exit) {
|
while (!do_exit) {
|
||||||
|
@ -47,7 +48,7 @@ void calibration_thread() {
|
||||||
extrinsic_matrix_eigen(i / 4, i % 4) = extrinsic_matrix[i];
|
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;
|
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(0) = camera_frame_from_road_frame.col(0);
|
||||||
camera_frame_from_ground.col(1) = camera_frame_from_road_frame.col(1);
|
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);
|
set_core_affinity(4);
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
|
bool wide_camera = false;
|
||||||
|
|
||||||
|
#ifdef QCOM2
|
||||||
|
wide_camera = Params().getBool("EnableWideCamera");
|
||||||
|
#endif
|
||||||
|
|
||||||
// start calibration thread
|
// start calibration thread
|
||||||
std::thread thread = std::thread(calibration_thread);
|
std::thread thread = std::thread(calibration_thread, wide_camera);
|
||||||
|
|
||||||
// cl init
|
// cl init
|
||||||
cl_device_id device_id = cl_get_device_id(CL_DEVICE_TYPE_DEFAULT);
|
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);
|
model_init(&model, device_id, context);
|
||||||
LOGW("models loaded, modeld starting");
|
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)) {
|
while (!do_exit && !vipc_client.connect(false)) {
|
||||||
util::sleep_for(100);
|
util::sleep_for(100);
|
||||||
}
|
}
|
||||||
|
|
|
@ -15,10 +15,10 @@
|
||||||
// TODO: choose based on frame input size
|
// TODO: choose based on frame input size
|
||||||
#ifdef QCOM2
|
#ifdef QCOM2
|
||||||
const float y_offset = 150.0;
|
const float y_offset = 150.0;
|
||||||
const float zoom = 1.1;
|
const float zoom = 2912.8;
|
||||||
#else
|
#else
|
||||||
const float y_offset = 0.0;
|
const float y_offset = 0.0;
|
||||||
const float zoom = 2.35;
|
const float zoom = 2138.5;
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
static void ui_draw_text(const UIState *s, float x, float y, const char *string, float size, NVGcolor color, const char *font_name) {
|
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));
|
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);
|
x = std::clamp(x, 0.f, s->viz_rect.right() - sz / 2);
|
||||||
y = std::fmin(s->viz_rect.bottom() - sz * .6, y);
|
y = std::fmin(s->viz_rect.bottom() - sz * .6, y);
|
||||||
draw_chevron(s, x, y, sz, nvgRGBA(201, 34, 49, fillAlpha), COLOR_YELLOW);
|
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);
|
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};
|
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 zx = s->zoom * 2 * intrinsic_matrix.v[2] / s->video_rect.w;
|
||||||
float zy = zoom * 2 * fcam_intrinsic_matrix.v[5] / s->video_rect.h;
|
float zy = s->zoom * 2 * intrinsic_matrix.v[5] / s->video_rect.h;
|
||||||
|
|
||||||
const mat4 frame_transform = {{
|
const mat4 frame_transform = {{
|
||||||
zx, 0.0, 0.0, 0.0,
|
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);
|
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
|
// 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
|
// 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);
|
nvgCurrentTransform(s->vg, s->car_space_transform);
|
||||||
nvgResetTransform(s->vg);
|
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.",
|
"In this mode openpilot will ignore lanelines and just drive how it thinks a human would.",
|
||||||
"../assets/offroad/icon_road.png"));
|
"../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");
|
bool record_lock = Params().getBool("RecordFrontLock");
|
||||||
record_toggle->setEnabled(!record_lock);
|
record_toggle->setEnabled(!record_lock);
|
||||||
|
|
||||||
|
|
|
@ -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 float margin = 500.0f;
|
||||||
const vec3 pt = (vec3){{in_x, in_y, in_z}};
|
const vec3 pt = (vec3){{in_x, in_y, in_z}};
|
||||||
const vec3 Ep = matvecmul3(s->scene.view_from_calib, pt);
|
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.
|
// Project.
|
||||||
float x = KEp.v[0] / KEp.v[2];
|
float x = KEp.v[0] / KEp.v[2];
|
||||||
|
@ -58,10 +58,17 @@ void ui_init(UIState *s) {
|
||||||
s->scene.started = false;
|
s->scene.started = false;
|
||||||
s->status = STATUS_OFFROAD;
|
s->status = STATUS_OFFROAD;
|
||||||
|
|
||||||
ui_nvg_init(s);
|
|
||||||
|
|
||||||
s->last_frame = nullptr;
|
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_front = new VisionIpcClient("camerad", VISION_STREAM_RGB_FRONT, true);
|
||||||
s->vipc_client = s->vipc_client_rear;
|
s->vipc_client = s->vipc_client_rear;
|
||||||
}
|
}
|
||||||
|
|
|
@ -167,6 +167,8 @@ typedef struct UIState {
|
||||||
bool sidebar_collapsed;
|
bool sidebar_collapsed;
|
||||||
Rect video_rect, viz_rect;
|
Rect video_rect, viz_rect;
|
||||||
float car_space_transform[6];
|
float car_space_transform[6];
|
||||||
|
bool wide_camera;
|
||||||
|
float zoom;
|
||||||
} UIState;
|
} UIState;
|
||||||
|
|
||||||
void ui_init(UIState *s);
|
void ui_init(UIState *s);
|
||||||
|
|
Loading…
Reference in New Issue