EuRoC playback functionality. More...

#include "xrt/xrt_frame.h"
#include "xrt/xrt_tracking.h"
#include "xrt/xrt_frameserver.h"
#include "os/os_threading.h"
#include "util/u_debug.h"
#include "util/u_misc.h"
#include "util/u_time.h"
#include "util/u_var.h"
#include "util/u_sink.h"
#include "tracking/t_frame_cv_mat_wrapper.hpp"
#include "math/m_api.h"
#include "math/m_filter_fifo.h"
#include "euroc_driver.h"
#include "euroc_interface.h"
#include <algorithm>
#include <chrono>
#include <cstring>
#include <stdint.h>
#include <stdio.h>
#include <fstream>
#include <future>
#include <thread>
#include <inttypes.h>

Include dependency graph for euroc_player.cpp:

Data Structures
struct	euroc_player
	Euroc player is in charge of the playback of a particular dataset. More...

Macros
#define	EUROC_PLAYER_STR "Euroc Player"

#define	EUROC_MAX_CAMS XRT_TRACKING_MAX_SLAM_CAMS
	Match max cameras to slam sinks max camera count. More...

#define	DEFINE_RECEIVE_CAM(cam_id)

Typedefs
using	img_sample = pair< timepoint_ns, string >

using	imu_samples = vector< xrt_imu_sample >

using	img_samples = vector< img_sample >

using	gt_trajectory = vector< xrt_pose_sample >

Enumerations
enum	euroc_player_ui_state { UNINITIALIZED = 0 , NOT_STREAMING , STREAM_PLAYING , STREAM_PAUSED , STREAM_ENDED }

Functions
static void	euroc_player_start_btn_cb (void *ptr)

static void	euroc_player_set_ui_state (struct euroc_player *ep, euroc_player_ui_state state)

static bool	euroc_player_preload_imu_data (const string &dataset_path, imu_samples *samples, int64_t read_n=-1)
	Parse and load all IMU samples into `samples`, assumes data.csv is well formed If `read_n` > 0, read at most that amount of samples Returns whether the appropriate data.csv file could be opened. More...

static bool	euroc_player_preload_gt_data (const string &dataset_path, const char *gtdev, gt_trajectory trajectory, int64_t read_n=-1)
	Parse and load ground truth device name and trajectory into `gtdev` and `trajectory` respectively. More...

static bool	euroc_player_preload_img_data (const string &dataset_path, img_samples &samples, size_t cam_id, int64_t read_n=-1)
	Parse and load image names and timestamps into `samples` If read_n > 0, read at most that amount of samples Returns whether the appropriate data.csv file could be opened. More...

static void	euroc_player_match_cams_seqs (struct euroc_player *ep)
	Trims cameras sequences so that they all start and end at the same sample Note that this function does not guarantee that the dataset is free of framedrops and it assumes it is properly formatted with monotonically increasing timestamps. More...

static void	euroc_player_preload (struct euroc_player *ep)

static void	euroc_player_user_skip (struct euroc_player *ep)
	Skips the first seconds of the dataset as specified by the user. More...

static void	euroc_player_fill_dataset_info (const char path, euroc_player_dataset_info dataset)
	Determine and fill attributes of the dataset pointed by `path` Assertion fails if `path` does not point to an euroc dataset. More...

static struct euroc_player *	euroc_player (struct xrt_fs *xfs)

static timepoint_ns	os_monotonic_get_ts ()
	Wrapper around os_monotonic_get_ns to convert to int64_t and check ranges. More...

static timepoint_ns	euroc_player_mapped_ts (struct euroc_player *ep, timepoint_ns ts)

static timepoint_ns	euroc_player_mapped_playback_ts (struct euroc_player *ep, timepoint_ns ts)
	Same as euroc_player_mapped_ts but only if playback options allow it. More...

static void	euroc_player_load_next_frame (struct euroc_player ep, int cam_index, struct xrt_frame &xf)

static void	euroc_player_push_next_frame (struct euroc_player *ep)

static void	euroc_player_push_next_imu (struct euroc_player *ep)

static void	euroc_player_push_all_gt (struct euroc_player *ep)

template<typename SamplesType >
timepoint_ns	euroc_player_get_next_euroc_ts (struct euroc_player *ep)

template<typename SamplesType >
void	euroc_player_sleep_until_next_sample (struct euroc_player *ep)

template<typename SamplesType >
auto	euroc_player_get_stream_set (struct euroc_player *ep)
	Based on the SamplesType to stream, return a set of corresponding entities: the samples vector, sequence number, push and sleep functions. More...

template<typename SamplesType >
static void	euroc_player_stream_samples (struct euroc_player *ep)

static void *	euroc_player_stream (void *ptr)

static bool	euroc_player_enumerate_modes (struct xrt_fs xfs, struct xrt_fs_mode out_modes, uint32_t out_count)

static bool	euroc_player_configure_capture (struct xrt_fs xfs, struct xrt_fs_capture_parameters cp)

static void	receive_cam0 (struct xrt_frame_sink sink, struct xrt_frame xf)

static void	receive_cam1 (struct xrt_frame_sink sink, struct xrt_frame xf)

static void	receive_cam2 (struct xrt_frame_sink sink, struct xrt_frame xf)

static void	receive_cam3 (struct xrt_frame_sink sink, struct xrt_frame xf)

static void	receive_cam4 (struct xrt_frame_sink sink, struct xrt_frame xf)

static void	receive_imu_sample (struct xrt_imu_sink sink, struct xrt_imu_sample s)

static bool	euroc_player_stream_start (struct xrt_fs xfs, struct xrt_frame_sink xs, enum xrt_fs_capture_type capture_type, uint32_t descriptor_index)
	This is the xrt_fs stream start method, however as the euroc playback is heavily customizable, it will be managed through the UI. More...

static bool	euroc_player_slam_stream_start (struct xrt_fs xfs, struct xrt_slam_sinks sinks)

static bool	euroc_player_stream_stop (struct xrt_fs *xfs)

static bool	euroc_player_is_running (struct xrt_fs *xfs)

static void	euroc_player_break_apart (struct xrt_frame_node *node)

static void	euroc_player_destroy (struct xrt_frame_node *node)

static void	euroc_player_pause_btn_cb (void *ptr)

static void	euroc_player_setup_gui (struct euroc_player *ep)

void	euroc_player_fill_default_config_for (struct euroc_player_config config, const char dataset_path)
	Fills in an euroc_player_config with defaults based on the provided dataset path. More...

struct xrt_fs *	euroc_player_create (struct xrt_frame_context xfctx, const char path, struct euroc_player_config *config)
	Create an euroc player from a path to a dataset. More...

Variables
static void(*	receive_cam [XRT_TRACKING_MAX_SLAM_CAMS])(struct xrt_frame_sink , struct xrt_frame )
	Be sure to define the same number of definition as EUROC_MAX_CAMS and to add them to `receive_cam`. More...

Detailed Description

EuRoC playback functionality.

Author: Mateo de Mayo mateo.nosp@m..dem.nosp@m.ayo@c.nosp@m.olla.nosp@m.bora..nosp@m.com

Macro Definition Documentation

◆ DEFINE_RECEIVE_CAM

#define DEFINE_RECEIVE_CAM ( cam_id )

Value:

        static void receive_cam##cam_id(struct xrt_frame_sink *sink, struct xrt_frame *xf)                             \
        {                                                                                                              \
                struct euroc_player *ep = container_of(sink, struct euroc_player, cam_sinks[cam_id]);                  \
                EUROC_TRACE(ep, "cam%d img t=%ld source_t=%ld", cam_id, xf->timestamp, xf->source_timestamp);          \
                u_sink_debug_push_frame(&ep->ui_cam_sinks[cam_id], xf);                                                \
                if (ep->out_sinks.cams[cam_id]) {                                                                      \
                        xrt_sink_push_frame(ep->out_sinks.cams[cam_id], xf);                                           \
                }                                                                                                      \
        }

◆ EUROC_MAX_CAMS

#define EUROC_MAX_CAMS XRT_TRACKING_MAX_SLAM_CAMS

Match max cameras to slam sinks max camera count.

◆ EUROC_PLAYER_STR

#define EUROC_PLAYER_STR "Euroc Player"

See also: euroc_player_playback_config

Function Documentation

◆ euroc_player_fill_dataset_info()

static void euroc_player_fill_dataset_info	(	const char *	path,
		euroc_player_dataset_info *	dataset
	)

static

Determine and fill attributes of the dataset pointed by path Assertion fails if path does not point to an euroc dataset.

References euroc_player_preload_img_data().

Referenced by euroc_player_fill_default_config_for().

◆ euroc_player_get_stream_set()

template<typename SamplesType >

auto euroc_player_get_stream_set ( struct euroc_player * ep )

Based on the SamplesType to stream, return a set of corresponding entities: the samples vector, sequence number, push and sleep functions.

References euroc_player::img_seq, euroc_player::imgs, euroc_player::imu_seq, and euroc_player::imus.

◆ euroc_player_load_next_frame()

static void euroc_player_load_next_frame	(	struct euroc_player *	ep,
		int	cam_index,
		struct xrt_frame *&	xf
	)

static

Todo:: Not using xrt_stereo_format because we use two sinks. It would probably be better to refactor everything to use stereo frames instead.

◆ euroc_player_mapped_playback_ts()

static timepoint_ns euroc_player_mapped_playback_ts	(	struct euroc_player *	ep,
		timepoint_ns	ts
	)

static

Same as euroc_player_mapped_ts but only if playback options allow it.

References euroc_player_mapped_ts(), euroc_player::playback, and euroc_player_playback_config::use_source_ts.

◆ euroc_player_mapped_ts()

static timepoint_ns euroc_player_mapped_ts	(	struct euroc_player *	ep,
		timepoint_ns	ts
	)

static

Returns: maps a dataset timestamp to current time

References euroc_player::base_ts, MAX, euroc_player::offset_ts, euroc_player::playback, euroc_player_playback_config::speed, and euroc_player::start_ts.

Referenced by euroc_player_mapped_playback_ts().

◆ euroc_player_match_cams_seqs()

static void euroc_player_match_cams_seqs ( struct euroc_player * ep )

static

Trims cameras sequences so that they all start and end at the same sample Note that this function does not guarantee that the dataset is free of framedrops and it assumes it is properly formatted with monotonically increasing timestamps.

References euroc_player::imgs.

◆ euroc_player_pause_btn_cb()

static void euroc_player_pause_btn_cb ( void * ptr )

static

Note: : if you have groundtruth, pausing will unsync it from the tracker.

References euroc_player::last_pause_ts, euroc_player::offset_ts, os_monotonic_get_ts(), euroc_player_playback_config::paused, and euroc_player::playback.

◆ euroc_player_preload_gt_data()

static bool euroc_player_preload_gt_data	(	const string &	dataset_path,
		const char **	gtdev,
		gt_trajectory *	trajectory,
		int64_t	read_n = `-1`
	)

static

Parse and load ground truth device name and trajectory into gtdev and trajectory respectively.

Parameters

[in]	dataset_path
[in,out]	gtdev	The name of the groundtruth device found in the dataset if any or `nullptr`. Groundtruth data can come from different devices, so we use the first of: The value prespecified in `gtdev` vicon0: found in euroc "vicon room" datasets mocap0: found in TUM-VI datasets with euroc format state_groundtruth_estimate0: found in euroc as a postprocessed ground truth (we only use first 7 columns) leica0: found in euroc "machine hall" datasets, only positional ground truth
[out]	trajectory	The read trajectory
[in]	read_n	If > 0, read at most that amount of gt poses

Returns: Whether the appropriate data.csv file could be opened

◆ euroc_player_preload_img_data()

static bool euroc_player_preload_img_data	(	const string &	dataset_path,
		img_samples &	samples,
		size_t	cam_id,
		int64_t	read_n = `-1`
	)

static

Parse and load image names and timestamps into samples If read_n > 0, read at most that amount of samples Returns whether the appropriate data.csv file could be opened.

Referenced by euroc_player_fill_dataset_info().

◆ euroc_player_preload_imu_data()

static bool euroc_player_preload_imu_data	(	const string &	dataset_path,
		imu_samples *	samples,
		int64_t	read_n = `-1`
	)

static

Parse and load all IMU samples into samples, assumes data.csv is well formed If read_n > 0, read at most that amount of samples Returns whether the appropriate data.csv file could be opened.

◆ euroc_player_stream_start()

static bool euroc_player_stream_start	(	struct xrt_fs *	xfs,
		struct xrt_frame_sink *	xs,
		enum xrt_fs_capture_type	capture_type,
		uint32_t	descriptor_index
	)

static

This is the xrt_fs stream start method, however as the euroc playback is heavily customizable, it will be managed through the UI.

So, unless EUROC_PLAY_FROM_START is set, this will not start outputting frames until the user clicks the start button.

◆ euroc_player_user_skip()

static void euroc_player_user_skip ( struct euroc_player * ep )

static

Skips the first seconds of the dataset as specified by the user.

References euroc_player::base_ts, euroc_player::img_seq, euroc_player::imgs, euroc_player::imu_seq, euroc_player::imus, MAX, euroc_player::offset_ts, euroc_player::playback, euroc_player_playback_config::skip_first, euroc_player_playback_config::skip_perc, euroc_player_playback_config::speed, and U_TIME_1S_IN_NS.

◆ os_monotonic_get_ts()

static timepoint_ns os_monotonic_get_ts ( )

static

Wrapper around os_monotonic_get_ns to convert to int64_t and check ranges.

References os_monotonic_get_ns().

Referenced by euroc_player_pause_btn_cb().

Variable Documentation

◆ receive_cam

void(* receive_cam[ XRT_TRACKING_MAX_SLAM_CAMS ])(struct xrt_frame_sink , struct xrt_frame )	(	struct xrt_frame_sink *	,
		struct xrt_frame *
	)

static

Initial value:

= {
    receive_cam0, 
    receive_cam1, 
    receive_cam2, 
    receive_cam3, 
    receive_cam4, 
}

Be sure to define the same number of definition as EUROC_MAX_CAMS and to add them to receive_cam.

Data Structures

Macros

Typedefs

Enumerations

Functions

Variables

Detailed Description

Macro Definition Documentation

◆ DEFINE_RECEIVE_CAM

◆ EUROC_MAX_CAMS

◆ EUROC_PLAYER_STR

Function Documentation

◆ euroc_player_fill_dataset_info()

◆ euroc_player_get_stream_set()

◆ euroc_player_load_next_frame()

◆ euroc_player_mapped_playback_ts()

◆ euroc_player_mapped_ts()

◆ euroc_player_match_cams_seqs()

◆ euroc_player_pause_btn_cb()

◆ euroc_player_preload_gt_data()

◆ euroc_player_preload_img_data()

◆ euroc_player_preload_imu_data()

◆ euroc_player_stream_start()

◆ euroc_player_user_skip()

◆ os_monotonic_get_ts()

Variable Documentation

◆ receive_cam