From 2cf07dedf2761fa4f0ab9bd90d3b356ef316c25a Mon Sep 17 00:00:00 2001 From: Mateo de Mayo Date: Mon, 23 May 2022 17:48:24 -0300 Subject: [PATCH] t/euroc: Fix crash when recording euroc dataset by providing UI button Previously, a crash occurred in some situations when the second sample was written before the first sample was able to create the necessary CSV files. --- .../auxiliary/tracking/t_euroc_recorder.cpp | 52 +++++++++++++++---- src/xrt/auxiliary/tracking/t_euroc_recorder.h | 12 ++++- src/xrt/auxiliary/tracking/t_tracker_slam.cpp | 19 ++----- 3 files changed, 59 insertions(+), 24 deletions(-) diff --git a/src/xrt/auxiliary/tracking/t_euroc_recorder.cpp b/src/xrt/auxiliary/tracking/t_euroc_recorder.cpp index a4cd143eb..f351c00bc 100644 --- a/src/xrt/auxiliary/tracking/t_euroc_recorder.cpp +++ b/src/xrt/auxiliary/tracking/t_euroc_recorder.cpp @@ -12,6 +12,7 @@ #include "os/os_time.h" #include "util/u_frame.h" #include "util/u_sink.h" +#include "util/u_var.h" #include #include @@ -33,8 +34,11 @@ using std::filesystem::create_directories; struct euroc_recorder { struct xrt_frame_node node; - string path; //!< Destination path for the dataset - bool first_received; //!< Whether we have received the first sample + string path; //!< Destination path for the dataset + + bool recording; //!< Whether samples are being recorded + bool files_created; //!< Whether the dataset directory structure has been created + struct u_var_button recording_btn; //!< UI button to start/stop `recording` // Cloner sinks: copy frame to heap for quick release of the original struct xrt_slam_sinks cloner_queues; //!< Queue sinks that write into cloner sinks @@ -67,11 +71,11 @@ struct euroc_recorder static void euroc_recorder_try_mkfiles(struct euroc_recorder *er) { - // Create directory structure and files only on first received frame - if (er->first_received) { + // Create directory structure and files only once + if (er->files_created) { return; } - er->first_received = true; + er->files_created = true; string path = er->path; @@ -110,7 +114,6 @@ extern "C" void euroc_recorder_save_imu(xrt_imu_sink *sink, struct xrt_imu_sample *sample) { euroc_recorder *er = container_of(sink, euroc_recorder, writer_imu_sink); - euroc_recorder_try_mkfiles(er); timepoint_ns ts = sample->timestamp_ns; xrt_vec3_f64 a = sample->accel_m_s2; @@ -124,8 +127,6 @@ euroc_recorder_save_imu(xrt_imu_sink *sink, struct xrt_imu_sample *sample) static void euroc_recorder_save_frame(euroc_recorder *er, struct xrt_frame *frame, bool is_left) { - euroc_recorder_try_mkfiles(er); - string cam_name = is_left ? "cam0" : "cam1"; uint64_t ts = frame->timestamp; @@ -169,6 +170,11 @@ euroc_recorder_receive_imu(xrt_imu_sink *sink, struct xrt_imu_sample *sample) // sinks so we use an std::queue to temporarily store IMU samples, later we // write them to disk when writing left frames. euroc_recorder *er = container_of(sink, euroc_recorder, cloner_imu_sink); + + if (!er->recording) { + return; + } + er->imu_queue.push(*sample); } @@ -176,6 +182,10 @@ euroc_recorder_receive_imu(xrt_imu_sink *sink, struct xrt_imu_sample *sample) static void euroc_recorder_receive_frame(euroc_recorder *er, struct xrt_frame *src_frame, bool is_left) { + if (!er->recording) { + return; + } + // Let's clone the frame so that we can release the src_frame quickly xrt_frame *copy = nullptr; u_frame_clone(src_frame, ©); @@ -228,10 +238,15 @@ euroc_recorder_node_destroy(struct xrt_frame_node *node) */ extern "C" xrt_slam_sinks * -euroc_recorder_create(struct xrt_frame_context *xfctx, const char *record_path) +euroc_recorder_create(struct xrt_frame_context *xfctx, const char *record_path, bool record_from_start) { struct euroc_recorder *er = new euroc_recorder{}; + er->recording = record_from_start; + if (record_from_start) { + euroc_recorder_try_mkfiles(er); + } + struct xrt_frame_node *xfn = &er->node; xfn->break_apart = euroc_recorder_node_break_apart; xfn->destroy = euroc_recorder_node_destroy; @@ -276,3 +291,22 @@ euroc_recorder_create(struct xrt_frame_context *xfctx, const char *record_path) xrt_slam_sinks *public_sinks = &er->cloner_queues; return public_sinks; } + +static void +euroc_recorder_btn_cb(void *ptr) +{ + euroc_recorder *er = (euroc_recorder *)ptr; + euroc_recorder_try_mkfiles(er); + er->recording = !er->recording; + snprintf(er->recording_btn.label, sizeof(er->recording_btn.label), + er->recording ? "Stop recording" : "Record EuRoC dataset"); +} + +extern "C" void +euroc_recorder_add_ui(struct xrt_slam_sinks *public_sinks, void *root) +{ + euroc_recorder *er = container_of(public_sinks, euroc_recorder, cloner_queues); + er->recording_btn.cb = euroc_recorder_btn_cb; + er->recording_btn.ptr = er; + u_var_add_button(root, &er->recording_btn, er->recording ? "Stop recording" : "Record EuRoC dataset"); +} diff --git a/src/xrt/auxiliary/tracking/t_euroc_recorder.h b/src/xrt/auxiliary/tracking/t_euroc_recorder.h index fe53a08c4..d402be786 100644 --- a/src/xrt/auxiliary/tracking/t_euroc_recorder.h +++ b/src/xrt/auxiliary/tracking/t_euroc_recorder.h @@ -24,12 +24,22 @@ extern "C" { * * @param xfctx Frame context for the sinks. * @param record_path Directory name to save the dataset or NULL for a default based on the current datetime. + * @param record_from_start Whether to start recording immediately on creation. * @return struct xrt_slam_sinks* Sinks to push samples to for recording. * * @ingroup aux_tracking */ struct xrt_slam_sinks * -euroc_recorder_create(struct xrt_frame_context *xfctx, const char *record_path); +euroc_recorder_create(struct xrt_frame_context *xfctx, const char *record_path, bool record_from_start); + +/*! + * Add EuRoC recorder UI button to start recording after creation. + * + * @param er The sinks returned by @ref euroc_recorder_create + * @param root The pointer to add UI button to + */ +void +euroc_recorder_add_ui(struct xrt_slam_sinks *er, void *root); #ifdef __cplusplus } diff --git a/src/xrt/auxiliary/tracking/t_tracker_slam.cpp b/src/xrt/auxiliary/tracking/t_tracker_slam.cpp index aa2e91184..e9c5a3569 100644 --- a/src/xrt/auxiliary/tracking/t_tracker_slam.cpp +++ b/src/xrt/auxiliary/tracking/t_tracker_slam.cpp @@ -302,7 +302,6 @@ struct TrackerSlam MatFrame *cv_wrapper; //!< Wraps a xrt_frame in a cv::Mat to send to the SLAM system struct xrt_slam_sinks *euroc_recorder; //!< EuRoC dataset recording sinks - bool euroc_record; //!< When true, samples will be saved to disk in EuRoC format // Used mainly for checking that the timestamps come in order timepoint_ns last_imu_ts = INT64_MIN; //!< Last received IMU sample timestamp @@ -748,8 +747,8 @@ setup_ui(TrackerSlam &t) u_var_add_root(&t, "SLAM Tracker", true); u_var_add_log_level(&t, &t.log_level, "Log Level"); u_var_add_bool(&t, &t.submit, "Submit data to SLAM"); - u_var_add_bool(&t, &t.euroc_record, "Record as EuRoC"); u_var_add_bool(&t, &t.gt.override_tracking, "Track with ground truth (if available)"); + euroc_recorder_add_ui(t.euroc_recorder, &t); u_var_add_gui_header(&t, NULL, "Trajectory Filter"); u_var_add_bool(&t, &t.filter.use_moving_average_filter, "Enable moving average filter"); @@ -850,9 +849,7 @@ t_slam_imu_sink_push(struct xrt_imu_sink *sink, struct xrt_imu_sample *s) } 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) { - xrt_sink_push_imu(t.euroc_recorder->imu, s); - } + xrt_sink_push_imu(t.euroc_recorder->imu, s); struct xrt_vec3 gyro = {(float)w.x, (float)w.y, (float)w.z}; struct xrt_vec3 accel = {(float)a.x, (float)a.y, (float)a.z}; @@ -890,10 +887,7 @@ 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); - - if (t.euroc_record) { - xrt_sink_push_frame(t.euroc_recorder->left, frame); - } + xrt_sink_push_frame(t.euroc_recorder->left, frame); } extern "C" void @@ -901,10 +895,7 @@ t_slam_frame_sink_push_right(struct xrt_frame_sink *sink, struct xrt_frame *fram { auto &t = *container_of(sink, TrackerSlam, right_sink); push_frame(t, frame, false); - - if (t.euroc_record) { - xrt_sink_push_frame(t.euroc_recorder->right, frame); - } + xrt_sink_push_frame(t.euroc_recorder->right, frame); } extern "C" void @@ -1039,7 +1030,7 @@ t_slam_create(struct xrt_frame_context *xfctx, xrt_frame_context_add(xfctx, &t.node); - t.euroc_recorder = euroc_recorder_create(xfctx, NULL); + t.euroc_recorder = euroc_recorder_create(xfctx, NULL, false); t.pred_type = config->prediction;