xrt: Support more than two cameras in a SLAM sink

This commit is contained in:
Mateo de Mayo 2023-02-10 16:24:49 -03:00 committed by Jakob Bornecrantz
parent a93dc650a1
commit 52cac31d3a
12 changed files with 90 additions and 80 deletions

View file

@ -274,7 +274,7 @@ euroc_recorder_receive_frame(euroc_recorder *er, struct xrt_frame *src_frame, bo
xrt_frame *copy = nullptr;
u_frame_clone(src_frame, &copy);
xrt_sink_push_frame(is_left ? er->writer_queues.left : er->writer_queues.right, copy);
xrt_sink_push_frame(is_left ? er->writer_queues.cams[0] : er->writer_queues.cams[1], copy);
xrt_frame_reference(&copy, NULL);
}
@ -357,8 +357,9 @@ euroc_recorder_create(struct xrt_frame_context *xfctx, const char *record_path,
// First, make the public queues that will clone frames in memory so that
// original frames can be released as soon as possible. Not doing this could
// result in frame queues from the user being filled up.
u_sink_queue_create(xfctx, 0, &er->cloner_left_sink, &er->cloner_queues.left);
u_sink_queue_create(xfctx, 0, &er->cloner_right_sink, &er->cloner_queues.right);
er->cloner_queues.cam_count = 2;
u_sink_queue_create(xfctx, 0, &er->cloner_left_sink, &er->cloner_queues.cams[0]);
u_sink_queue_create(xfctx, 0, &er->cloner_right_sink, &er->cloner_queues.cams[1]);
er->cloner_queues.imu = &er->cloner_imu_sink;
er->cloner_queues.gt = &er->cloner_gt_sink;
@ -369,8 +370,9 @@ euroc_recorder_create(struct xrt_frame_context *xfctx, const char *record_path,
er->cloner_right_sink.push_frame = euroc_recorder_receive_right;
// Then, make a queue to save frame sinks to disk in a separate thread
u_sink_queue_create(xfctx, 0, &er->writer_left_sink, &er->writer_queues.left);
u_sink_queue_create(xfctx, 0, &er->writer_right_sink, &er->writer_queues.right);
er->writer_queues.cam_count = 2;
u_sink_queue_create(xfctx, 0, &er->writer_left_sink, &er->writer_queues.cams[0]);
u_sink_queue_create(xfctx, 0, &er->writer_right_sink, &er->writer_queues.cams[1]);
er->writer_queues.imu = nullptr;
er->writer_queues.gt = nullptr;

View file

@ -1224,18 +1224,20 @@ extern "C" void
t_slam_frame_sink_push_left(struct xrt_frame_sink *sink, struct xrt_frame *frame)
{
auto &t = *container_of(sink, TrackerSlam, left_sink);
push_frame(t, frame, true);
int cam_id = 0;
push_frame(t, frame, cam_id);
u_sink_debug_push_frame(&t.ui_left_sink, frame);
xrt_sink_push_frame(t.euroc_recorder->left, frame);
xrt_sink_push_frame(t.euroc_recorder->cams[0], frame);
}
extern "C" void
t_slam_frame_sink_push_right(struct xrt_frame_sink *sink, struct xrt_frame *frame)
{
auto &t = *container_of(sink, TrackerSlam, right_sink);
push_frame(t, frame, false);
int cam_id = 1;
push_frame(t, frame, cam_id);
u_sink_debug_push_frame(&t.ui_right_sink, frame);
xrt_sink_push_frame(t.euroc_recorder->right, frame);
xrt_sink_push_frame(t.euroc_recorder->cams[1], frame);
}
extern "C" void
@ -1372,8 +1374,9 @@ t_slam_create(struct xrt_frame_context *xfctx,
t.imu_sink.push_imu = t_slam_imu_sink_push;
t.gt_sink.push_pose = t_slam_gt_sink_push;
t.sinks.left = &t.left_sink;
t.sinks.right = &t.right_sink;
t.sinks.cam_count = NUM_CAMS;
t.sinks.cams[0] = &t.left_sink;
t.sinks.cams[1] = &t.right_sink;
t.sinks.imu = &t.imu_sink;
t.sinks.gt = &t.gt_sink;

View file

@ -463,9 +463,9 @@ euroc_player_push_next_frame(struct euroc_player *ep)
}
ep->img_seq++;
xrt_sink_push_frame(ep->in_sinks.left, left_xf);
xrt_sink_push_frame(ep->in_sinks.cams[0], left_xf);
if (stereo) {
xrt_sink_push_frame(ep->in_sinks.right, right_xf);
xrt_sink_push_frame(ep->in_sinks.cams[1], right_xf);
}
xrt_frame_reference(&left_xf, NULL);
@ -662,8 +662,8 @@ receive_left_frame(struct xrt_frame_sink *sink, struct xrt_frame *xf)
struct euroc_player *ep = container_of(sink, struct euroc_player, left_sink);
EUROC_TRACE(ep, "left img t=%ld source_t=%ld", xf->timestamp, xf->source_timestamp);
u_sink_debug_push_frame(&ep->ui_left_sink, xf);
if (ep->out_sinks.left) {
xrt_sink_push_frame(ep->out_sinks.left, xf);
if (ep->out_sinks.cams[0]) {
xrt_sink_push_frame(ep->out_sinks.cams[0], xf);
}
}
@ -673,8 +673,8 @@ receive_right_frame(struct xrt_frame_sink *sink, struct xrt_frame *xf)
struct euroc_player *ep = container_of(sink, struct euroc_player, right_sink);
EUROC_TRACE(ep, "right img t=%ld source_t=%ld", xf->timestamp, xf->source_timestamp);
u_sink_debug_push_frame(&ep->ui_right_sink, xf);
if (ep->out_sinks.right) {
xrt_sink_push_frame(ep->out_sinks.right, xf);
if (ep->out_sinks.cams[1]) {
xrt_sink_push_frame(ep->out_sinks.cams[1], xf);
}
}
@ -714,7 +714,7 @@ euroc_player_stream_start(struct xrt_fs *xfs,
if (xs == NULL && capture_type == XRT_FS_CAPTURE_TYPE_TRACKING) {
EUROC_INFO(ep, "Starting Euroc Player in tracking mode");
if (ep->out_sinks.left == NULL) {
if (ep->out_sinks.cams[0] == NULL) {
EUROC_WARN(ep, "No left sink provided, will keep running but tracking is unlikely to work");
}
if (ep->playback.play_from_start) {
@ -722,7 +722,7 @@ euroc_player_stream_start(struct xrt_fs *xfs,
}
} else if (xs != NULL && capture_type == XRT_FS_CAPTURE_TYPE_CALIBRATION) {
EUROC_INFO(ep, "Starting Euroc Player in calibration mode, will stream only left frames right away");
ep->out_sinks.left = xs;
ep->out_sinks.cams[0] = xs;
euroc_player_start_btn_cb(ep);
} else {
EUROC_ASSERT(false, "Unsupported stream configuration xs=%p capture_type=%d", (void *)xs, capture_type);
@ -970,10 +970,10 @@ euroc_player_create(struct xrt_frame_context *xfctx, const char *path, struct eu
ep->left_sink.push_frame = receive_left_frame;
ep->right_sink.push_frame = receive_right_frame;
ep->imu_sink.push_imu = receive_imu_sample;
ep->in_sinks.left = &ep->left_sink;
ep->in_sinks.right = &ep->right_sink;
ep->in_sinks.cam_count = 2;
ep->in_sinks.cams[0] = &ep->left_sink;
ep->in_sinks.cams[1] = &ep->right_sink;
ep->in_sinks.imu = &ep->imu_sink;
ep->out_sinks = {0, 0, 0, 0};
struct xrt_fs *xfs = &ep->base;
xfs->enumerate_modes = euroc_player_enumerate_modes;

View file

@ -638,8 +638,8 @@ handle_frameset(struct rs_source *rs, rs2_frame *frames)
xf_left->timestamp = ts;
xf_right->timestamp = ts;
xrt_sink_push_frame(rs->in_sinks.left, xf_left);
xrt_sink_push_frame(rs->in_sinks.right, xf_right);
xrt_sink_push_frame(rs->in_sinks.cams[0], xf_left);
xrt_sink_push_frame(rs->in_sinks.cams[1], xf_right);
} else {
// This usually happens only once at start and never again
RS_WARN(rs, "Realsense device sent left and right frames with different timestamps %ld != %ld",
@ -648,7 +648,7 @@ handle_frameset(struct rs_source *rs, rs2_frame *frames)
xrt_frame_reference(&xf_right, NULL);
} else {
xrt_sink_push_frame(rs->in_sinks.left, xf_left);
xrt_sink_push_frame(rs->in_sinks.cams[0], xf_left);
}
xrt_frame_reference(&xf_left, NULL);
@ -872,11 +872,12 @@ rs_source_stream_start(struct xrt_fs *xfs,
{
struct rs_source *rs = rs_source_from_xfs(xfs);
if (xs == NULL && capture_type == XRT_FS_CAPTURE_TYPE_TRACKING) {
RS_ASSERT(rs->out_sinks.left != NULL, "No left sink provided");
RS_ASSERT(rs->out_sinks.cams[0] != NULL, "No left sink provided");
RS_INFO(rs, "Starting RealSense stream in tracking mode");
} else if (xs != NULL && capture_type == XRT_FS_CAPTURE_TYPE_CALIBRATION) {
RS_INFO(rs, "Starting RealSense stream in calibration mode, will stream only left frames");
rs->out_sinks.left = xs;
rs->out_sinks.cam_count = 1;
rs->out_sinks.cams[0] = xs;
} else {
RS_ASSERT(false, "Unsupported stream configuration xs=%p capture_type=%d", (void *)xs, capture_type);
return false;
@ -915,8 +916,8 @@ receive_left_frame(struct xrt_frame_sink *sink, struct xrt_frame *xf)
struct rs_source *rs = container_of(sink, struct rs_source, left_sink);
RS_TRACE(rs, "left img t=%ld source_t=%ld", xf->timestamp, xf->source_timestamp);
u_sink_debug_push_frame(&rs->ui_left_sink, xf);
if (rs->out_sinks.left) {
xrt_sink_push_frame(rs->out_sinks.left, xf);
if (rs->out_sinks.cams[0]) {
xrt_sink_push_frame(rs->out_sinks.cams[0], xf);
}
}
@ -926,8 +927,8 @@ receive_right_frame(struct xrt_frame_sink *sink, struct xrt_frame *xf)
struct rs_source *rs = container_of(sink, struct rs_source, right_sink);
RS_TRACE(rs, "right img t=%ld source_t=%ld", xf->timestamp, xf->source_timestamp);
u_sink_debug_push_frame(&rs->ui_right_sink, xf);
if (rs->out_sinks.right) {
xrt_sink_push_frame(rs->out_sinks.right, xf);
if (rs->out_sinks.cams[1]) {
xrt_sink_push_frame(rs->out_sinks.cams[1], xf);
}
}
@ -1096,8 +1097,9 @@ rs_source_create(struct xrt_frame_context *xfctx, int device_idx)
rs->left_sink.push_frame = receive_left_frame;
rs->right_sink.push_frame = receive_right_frame;
rs->imu_sink.push_imu = receive_imu_sample;
rs->in_sinks.left = &rs->left_sink;
rs->in_sinks.right = &rs->right_sink;
rs->in_sinks.cam_count = 2;
rs->in_sinks.cams[0] = &rs->left_sink;
rs->in_sinks.cams[1] = &rs->right_sink;
rs->in_sinks.imu = &rs->imu_sink;
// Prepare UI

View file

@ -419,15 +419,13 @@ rift_s_tracker_create(struct xrt_tracking_origin *origin,
struct xrt_frame_sink *entry_left_sink = NULL;
struct xrt_frame_sink *entry_right_sink = NULL;
u_sink_split_create(xfctx, slam_sinks->left, hand_sinks->left, &entry_left_sink);
u_sink_split_create(xfctx, slam_sinks->right, hand_sinks->right, &entry_right_sink);
u_sink_split_create(xfctx, slam_sinks->cams[0], hand_sinks->cams[0], &entry_left_sink);
u_sink_split_create(xfctx, slam_sinks->cams[1], hand_sinks->cams[1], &entry_right_sink);
entry_sinks = (struct xrt_slam_sinks){
.left = entry_left_sink,
.right = entry_right_sink,
.imu = slam_sinks->imu,
.gt = slam_sinks->gt,
};
entry_sinks = *slam_sinks;
entry_sinks.cam_count = 2;
entry_sinks.cams[0] = entry_left_sink;
entry_sinks.cams[1] = entry_right_sink;
} else if (slam_enabled) {
entry_sinks = *slam_sinks;
} else if (hand_enabled) {
@ -595,14 +593,14 @@ rift_s_tracker_push_slam_frames(struct rift_s_tracker *t,
t->last_frame_time = frame_time;
os_mutex_unlock(&t->mutex);
if (t->slam_sinks.left) {
if (t->slam_sinks.cams[0]) {
left_frame->timestamp = frame_time;
xrt_sink_push_frame(t->slam_sinks.left, left_frame);
xrt_sink_push_frame(t->slam_sinks.cams[0], left_frame);
}
if (t->slam_sinks.right) {
if (t->slam_sinks.cams[1]) {
right_frame->timestamp = frame_time;
xrt_sink_push_frame(t->slam_sinks.right, right_frame);
xrt_sink_push_frame(t->slam_sinks.cams[1], right_frame);
}
}

View file

@ -128,8 +128,8 @@ vive_source_receive_sbs_frame(struct xrt_frame_sink *sink, struct xrt_frame *xf)
VIVE_TRACE(vs, "sbs img t=%ld source_t=%ld", xf->timestamp, xf->source_timestamp);
if (vs->out_sinks.left) { // The split into left right will happen downstream
xrt_sink_push_frame(vs->out_sinks.left, xf);
if (vs->out_sinks.cams[0]) { // The split into left right will happen downstream
xrt_sink_push_frame(vs->out_sinks.cams[0], xf);
}
}
@ -178,8 +178,8 @@ vive_source_create(struct xrt_frame_context *xfctx)
// Setup sinks
vs->sbs_sink.push_frame = vive_source_receive_sbs_frame;
vs->imu_sink.push_imu = vive_source_receive_imu_sample;
vs->in_sinks.left = &vs->sbs_sink;
vs->in_sinks.right = NULL;
vs->in_sinks.cam_count = 1;
vs->in_sinks.cams[0] = &vs->sbs_sink;
vs->in_sinks.imu = &vs->imu_sink;
vs->timestamps_have_been_zero_until_now = true;
@ -219,5 +219,6 @@ void
vive_source_hook_into_sinks(struct vive_source *vs, struct xrt_slam_sinks *sinks)
{
vs->out_sinks = *sinks;
sinks->left = vs->in_sinks.left;
sinks->cam_count = 1;
sinks->cams[0] = vs->in_sinks.cams[0];
}

View file

@ -1708,15 +1708,13 @@ wmr_hmd_setup_trackers(struct wmr_hmd *wh, struct xrt_slam_sinks *out_sinks, str
struct xrt_frame_sink *entry_left_sink = NULL;
struct xrt_frame_sink *entry_right_sink = NULL;
u_sink_split_create(&wh->tracking.xfctx, slam_sinks->left, hand_sinks->left, &entry_left_sink);
u_sink_split_create(&wh->tracking.xfctx, slam_sinks->right, hand_sinks->right, &entry_right_sink);
u_sink_split_create(&wh->tracking.xfctx, slam_sinks->cams[0], hand_sinks->cams[0], &entry_left_sink);
u_sink_split_create(&wh->tracking.xfctx, slam_sinks->cams[1], hand_sinks->cams[1], &entry_right_sink);
entry_sinks = (struct xrt_slam_sinks){
.left = entry_left_sink,
.right = entry_right_sink,
.imu = slam_sinks->imu,
.gt = slam_sinks->gt,
};
entry_sinks = *slam_sinks;
entry_sinks.cam_count = 2;
entry_sinks.cams[0] = entry_left_sink;
entry_sinks.cams[1] = entry_right_sink;
} else if (slam_enabled) {
entry_sinks = *slam_sinks;
} else if (hand_enabled) {

View file

@ -93,8 +93,8 @@ receive_left_frame(struct xrt_frame_sink *sink, struct xrt_frame *xf)
xf->timestamp += ws->cam_hw2mono;
WMR_TRACE(ws, "left img t=%ld source_t=%ld", xf->timestamp, xf->source_timestamp);
u_sink_debug_push_frame(&ws->ui_left_sink, xf);
if (ws->out_sinks.left && ws->first_imu_received) {
xrt_sink_push_frame(ws->out_sinks.left, xf);
if (ws->out_sinks.cams[0] && ws->first_imu_received) {
xrt_sink_push_frame(ws->out_sinks.cams[0], xf);
}
}
@ -105,8 +105,8 @@ receive_right_frame(struct xrt_frame_sink *sink, struct xrt_frame *xf)
xf->timestamp += ws->cam_hw2mono;
WMR_TRACE(ws, "right img t=%ld source_t=%ld", xf->timestamp, xf->source_timestamp);
u_sink_debug_push_frame(&ws->ui_right_sink, xf);
if (ws->out_sinks.right && ws->first_imu_received) {
xrt_sink_push_frame(ws->out_sinks.right, xf);
if (ws->out_sinks.cams[1] && ws->first_imu_received) {
xrt_sink_push_frame(ws->out_sinks.cams[1], xf);
}
}
@ -206,7 +206,8 @@ wmr_source_stream_start(struct xrt_fs *xfs,
WMR_INFO(ws, "Starting WMR stream in tracking mode");
} else if (xs != NULL && capture_type == XRT_FS_CAPTURE_TYPE_CALIBRATION) {
WMR_INFO(ws, "Starting WMR stream in calibration mode, will stream only left frames");
ws->out_sinks.left = xs;
ws->out_sinks.cam_count = 1;
ws->out_sinks.cams[0] = xs;
} else {
WMR_ASSERT(false, "Unsupported stream configuration xs=%p capture_type=%d", (void *)xs, capture_type);
return false;
@ -302,10 +303,12 @@ wmr_source_create(struct xrt_frame_context *xfctx, struct xrt_prober_device *dev
ws->left_sink.push_frame = receive_left_frame;
ws->right_sink.push_frame = receive_right_frame;
ws->imu_sink.push_imu = receive_imu_sample;
ws->in_sinks.left = &ws->left_sink;
ws->in_sinks.right = &ws->right_sink;
ws->in_sinks.cam_count = 2;
ws->in_sinks.cams[0] = &ws->left_sink;
ws->in_sinks.cams[1] = &ws->right_sink;
ws->in_sinks.imu = &ws->imu_sink;
ws->camera = wmr_camera_open(dev_holo, ws->in_sinks.left, ws->in_sinks.right, cfg.n_cameras, ws->log_level);
ws->camera =
wmr_camera_open(dev_holo, ws->in_sinks.cams[0], ws->in_sinks.cams[1], cfg.n_cameras, ws->log_level);
ws->config = cfg;
// Setup UI

View file

@ -10,6 +10,7 @@
#pragma once
#define XRT_TRACKING_NAME_LEN 256
#define XRT_TRACKING_MAX_SLAM_CAMS 5
#include "xrt/xrt_defines.h"
@ -173,8 +174,8 @@ struct xrt_pose_sink
*/
struct xrt_slam_sinks
{
struct xrt_frame_sink *left;
struct xrt_frame_sink *right;
int cam_count;
struct xrt_frame_sink *cams[XRT_TRACKING_MAX_SLAM_CAMS];
struct xrt_imu_sink *imu;
struct xrt_pose_sink *gt; //!< Can receive ground truth poses if available
};

View file

@ -354,7 +354,8 @@ scene_render_select(struct gui_scene *scene, struct gui_program *p)
if (cs->settings->camera_type == XRT_SETTINGS_CAMERA_TYPE_SLAM) {
struct xrt_frame_sink *tmp = cali;
struct xrt_slam_sinks sinks;
u_sink_combiner_create(cs->xfctx, tmp, &sinks.left, &sinks.right);
sinks.cam_count = 2;
u_sink_combiner_create(cs->xfctx, tmp, &sinks.cams[0], &sinks.cams[1]);
xrt_fs_slam_stream_start(cs->xfs, &sinks);
} else {

View file

@ -416,20 +416,20 @@ valve_index_setup_visual_trackers(struct lighthouse_system *lhs,
struct xrt_frame_sink *entry_sbs_sink = NULL;
if (slam_enabled && hand_enabled) {
u_sink_split_create(&lhs->devices->xfctx, slam_sinks->left, hand_sinks->left, &entry_left_sink);
u_sink_split_create(&lhs->devices->xfctx, slam_sinks->right, hand_sinks->right, &entry_right_sink);
u_sink_split_create(&lhs->devices->xfctx, slam_sinks->cams[0], hand_sinks->cams[0], &entry_left_sink);
u_sink_split_create(&lhs->devices->xfctx, slam_sinks->cams[1], hand_sinks->cams[1], &entry_right_sink);
u_sink_stereo_sbs_to_slam_sbs_create(&lhs->devices->xfctx, entry_left_sink, entry_right_sink,
&entry_sbs_sink);
u_sink_create_format_converter(&lhs->devices->xfctx, XRT_FORMAT_L8, entry_sbs_sink, &entry_sbs_sink);
} else if (slam_enabled) {
entry_left_sink = slam_sinks->left;
entry_right_sink = slam_sinks->right;
entry_left_sink = slam_sinks->cams[0];
entry_right_sink = slam_sinks->cams[1];
u_sink_stereo_sbs_to_slam_sbs_create(&lhs->devices->xfctx, entry_left_sink, entry_right_sink,
&entry_sbs_sink);
u_sink_create_format_converter(&lhs->devices->xfctx, XRT_FORMAT_L8, entry_sbs_sink, &entry_sbs_sink);
} else if (hand_enabled) {
entry_left_sink = hand_sinks->left;
entry_right_sink = hand_sinks->right;
entry_left_sink = hand_sinks->cams[0];
entry_right_sink = hand_sinks->cams[1];
u_sink_stereo_sbs_to_slam_sbs_create(&lhs->devices->xfctx, entry_left_sink, entry_right_sink,
&entry_sbs_sink);
u_sink_create_format_converter(&lhs->devices->xfctx, XRT_FORMAT_L8, entry_sbs_sink, &entry_sbs_sink);
@ -441,8 +441,8 @@ valve_index_setup_visual_trackers(struct lighthouse_system *lhs,
u_sink_simple_queue_create(&lhs->devices->xfctx, entry_sbs_sink, &entry_sbs_sink);
struct xrt_slam_sinks entry_sinks = {
.left = entry_sbs_sink,
.right = NULL, // v4l2 streams a single SBS frame so we ignore the right sink
.cam_count = 1,
.cams = {entry_sbs_sink},
.imu = slam_enabled ? slam_sinks->imu : NULL,
.gt = slam_enabled ? slam_sinks->gt : NULL,
};
@ -479,7 +479,7 @@ stream_data_sources(struct lighthouse_system *lhs, struct xrt_prober *xp, struct
vive_source_hook_into_sinks(vs, &sinks);
}
success = xrt_fs_stream_start(lhs->xfs, sinks.left, XRT_FS_CAPTURE_TYPE_TRACKING, mode);
success = xrt_fs_stream_start(lhs->xfs, sinks.cams[0], XRT_FS_CAPTURE_TYPE_TRACKING, mode);
if (!success) {
LH_ERROR("Unable to start data streaming");

View file

@ -233,8 +233,9 @@ t_hand_tracking_async_default_create(struct xrt_frame_context *xfctx, struct t_h
struct ht_async_impl *hta = U_TYPED_CALLOC(struct ht_async_impl);
hta->base.left.push_frame = ht_async_receive_left;
hta->base.right.push_frame = ht_async_receive_right;
hta->base.sinks.left = &hta->base.left;
hta->base.sinks.right = &hta->base.right;
hta->base.sinks.cam_count = 2;
hta->base.sinks.cams[0] = &hta->base.left;
hta->base.sinks.cams[1] = &hta->base.right;
hta->base.node.break_apart = ht_async_break_apart;
hta->base.node.destroy = ht_async_destroy;
hta->base.get_hand = ht_async_get_hand;