mirrors/monado
1
0
Fork 0
mirror of https://gitlab.freedesktop.org/monado/monado.git synced 2025-01-17 04:15:44 +00:00
Code Issues Actions Packages Projects Releases Wiki Activity

t/slam: Add SLAM_SUBMIT_FROM_START envvar

Browse source 
Useful for setups that need some tuning to their sensors before sending
data to a SLAM system.
Removes the submit_frames property from wmr_camera as this replaces it.
This commit is contained in:
Mateo de Mayo 2021-12-14 15:59:47 -03:00 committed by Jakob Bornecrantz
parent 1c7323f1c2
commit 74e149159b
2 changed files with 14 additions and 10 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

11
src/xrt/auxiliary/tracking/t_tracker_slam.cpp
View file

@ -51,6 +51,9 @@ DEBUG_GET_ONCE_LOG_OPTION(slam_log, "SLAM_LOG", U_LOGGING_WARN)
//! Config file path, format is specific to the SLAM implementation in use
DEBUG_GET_ONCE_OPTION(slam_config, "SLAM_CONFIG", NULL)
//! Whether to submit data to the SLAM tracker without user action
DEBUG_GET_ONCE_BOOL_OPTION(slam_submit_from_start, "SLAM_SUBMIT_FROM_START", true)
//! Namespace for the interface to the external SLAM tracking system
namespace xrt::auxiliary::tracking::slam {
@ -169,6 +172,7 @@ struct TrackerSlam
struct xrt_frame_sink left_sink = {}; //!< Sends left camera frames to the SLAM system
struct xrt_frame_sink right_sink = {}; //!< Sends right camera frames to the SLAM system
struct xrt_imu_sink imu_sink = {}; //!< Sends imu samples to the SLAM system
bool submit; //!< Whether to submit data pushed to sinks to the SLAM tracker
enum u_logging_level log_level; //!< Logging level for the SLAM tracker, set by SLAM_LOG var
struct os_thread_helper oth; //!< Thread where the external SLAM system runs
@ -200,7 +204,9 @@ t_slam_imu_sink_push(struct xrt_imu_sink *sink, struct xrt_imu_sample *s)
//! @todo There are many conversions like these between xrt and
//! slam_tracker.hpp types. Implement a casting mechanism to avoid copies.
imu_sample sample{ts, a.x, a.y, a.z, w.x, w.y, w.z};
if (t.submit) {
t.slam->push_imu_sample(sample);
}
SLAM_TRACE("imu t=%ld a=[%f,%f,%f] w=[%f,%f,%f]", ts, a.x, a.y, a.z, w.x, w.y, w.z);
if (t.euroc_record) {
@ -247,7 +253,9 @@ push_frame(const TrackerSlam &t, struct xrt_frame *frame, bool is_left)
cv::Mat img = t.cv_wrapper->wrap(frame);
SLAM_DASSERT_(frame->timestamp < INT64_MAX);
img_sample sample{(int64_t)frame->timestamp, img, is_left};
if (t.submit) {
t.slam->push_frame(sample);
}
SLAM_TRACE("%s frame t=%lu", is_left ? " left" : "right", frame->timestamp);
// Check monotonically increasing timestamps
@ -352,6 +360,8 @@ t_slam_create(struct xrt_frame_context *xfctx, struct xrt_tracked_slam **out_xts
t.sinks.right = &t.right_sink;
t.sinks.imu = &t.imu_sink;
t.submit = debug_get_bool_option_slam_submit_from_start();
t.node.break_apart = t_slam_node_break_apart;
t.node.destroy = t_slam_node_destroy;
@ -364,6 +374,7 @@ t_slam_create(struct xrt_frame_context *xfctx, struct xrt_tracked_slam **out_xts
// Setup UI
u_var_add_root(&t, "SLAM Tracker", true);
u_var_add_bool(&t, &t.submit, "Submit data to SLAM");
u_var_add_bool(&t, &t.euroc_record, "Record as EuRoC");
*out_xts = &t.base;

9
src/xrt/drivers/wmr/wmr_camera.c
View file

@ -96,7 +96,6 @@ struct wmr_camera
struct u_sink_debug debug_sinks[2];
bool submit_frames; //!< Whether to push frames to left/right sinks
struct xrt_frame_sink *left_sink; //!< Downstream sinks to push left tracking frames to
struct xrt_frame_sink *right_sink; //!< Downstream sinks to push right tracking frames to
@ -344,7 +343,7 @@ img_xfer_cb(struct libusb_transfer *xfer)
// Push to sinks
bool tracking_frame = sink_index == 0;
if (tracking_frame && cam->submit_frames) {
if (tracking_frame) {
// Tracking frames usually come at ~30fps
struct xrt_frame *xf_left = NULL;
struct xrt_frame *xf_right = NULL;
@ -401,11 +400,6 @@ wmr_camera_open(struct xrt_prober_device *dev_holo,
cam->log_level = log_level;
cam->debug_gain = DEFAULT_GAIN;
cam->debug_exposure = DEFAULT_EXPOSURE / UI_EXPOSURE_STEP_SIZE;
//! @todo downstream sinks are currently being used for SLAM and they need the
//! exposure and gain to be well set by the user before receiving frames. So
//! we don't submit frames right away. It would be best if the exposure and
//! gain were automatically set.
cam->submit_frames = false;
if (os_thread_helper_init(&cam->usb_thread) != 0) {
WMR_CAM_ERROR(cam, "Failed to initialise threading");
@ -448,7 +442,6 @@ wmr_camera_open(struct xrt_prober_device *dev_holo,
u_var_add_log_level(cam, &cam->log_level, "Log level");
u_var_add_u8(cam, &cam->debug_gain, "Gain");
u_var_add_u8(cam, &cam->debug_exposure, "Exposure * 200");
u_var_add_bool(cam, &cam->submit_frames, "Submit frames");
u_var_add_sink_debug(cam, &cam->debug_sinks[0], "Tracking Streams");
u_var_add_sink_debug(cam, &cam->debug_sinks[1], "Controller Streams");