mirror of
https://gitlab.freedesktop.org/monado/monado.git
synced 2024-12-28 02:26:16 +00:00
d/euroc: Add EuRoC dataset reader driver.
A squash of the following commits. d/euroc: Add Euroc driver initial boilerplate d/euroc: Implement xrt_frame_node interface d/euroc: Implement mainloop thread start flow d/euroc: Parse samples from data.csv files d/euroc: Determine dataset information at start Also use that information to provide a xrt_fs mode and to know if right camera images are available to use. d/euroc: Produce frames for left and right sinks d/euroc: Implement xrt_imu_sink d/euroc: Implement playback properties d/euroc: Implement pausing with UI state machine d/euroc: Show IMU and progress data in UI d/euroc: Make sure to give in nullptr or valid pointers to wrapL8 d/euroc: Unreference frames when done with them d/euroc: Fix leaks with debug sinks, and work around free issue d/euroc: Refactor playback to produce samples with current timestamps d/euroc: Fix double free by separating debug sinks from downstream sinks
This commit is contained in:
Mateo de Mayo
Jakob Bornecrantz
parent
00a2f891a2
commit
97c59bd59f
|
|
@ -214,6 +214,7 @@ cmake_dependent_option(XRT_BUILD_DRIVER_SURVIVE "Enable libsurvive driver" ON "S
|
||||||
|
|
||||||
cmake_dependent_option(XRT_BUILD_DRIVER_ANDROID "Enable Android sensors driver" ON "ANDROID" OFF)
|
cmake_dependent_option(XRT_BUILD_DRIVER_ANDROID "Enable Android sensors driver" ON "ANDROID" OFF)
|
||||||
cmake_dependent_option(XRT_BUILD_DRIVER_QWERTY "Enable Qwerty driver" ON "XRT_HAVE_SDL2" OFF)
|
cmake_dependent_option(XRT_BUILD_DRIVER_QWERTY "Enable Qwerty driver" ON "XRT_HAVE_SDL2" OFF)
|
||||||
|
cmake_dependent_option(XRT_BUILD_DRIVER_EUROC "Enable EuRoC dataset driver for SLAM evaluation" ON "XRT_HAVE_OPENCV" OFF)
|
||||||
|
|
||||||
# You can set this from a superproject to add a driver
|
# You can set this from a superproject to add a driver
|
||||||
# All drivers must be listed in here to be included in the generated header!
|
# All drivers must be listed in here to be included in the generated header!
|
||||||
|
|
@ -240,6 +241,7 @@ list(APPEND AVAILABLE_DRIVERS
|
||||||
"VIVE"
|
"VIVE"
|
||||||
"QWERTY"
|
"QWERTY"
|
||||||
"WMR"
|
"WMR"
|
||||||
|
"EUROC"
|
||||||
)
|
)
|
||||||
|
|
||||||
|
|
||||||
|
|
@ -393,6 +395,7 @@ message(STATUS "# DRIVER_DEPTHAI: ${XRT_BUILD_DRIVER_DEPTHAI}")
|
||||||
message(STATUS "# DRIVER_VIVE: ${XRT_BUILD_DRIVER_VIVE}")
|
message(STATUS "# DRIVER_VIVE: ${XRT_BUILD_DRIVER_VIVE}")
|
||||||
message(STATUS "# DRIVER_QWERTY: ${XRT_BUILD_DRIVER_QWERTY}")
|
message(STATUS "# DRIVER_QWERTY: ${XRT_BUILD_DRIVER_QWERTY}")
|
||||||
message(STATUS "# DRIVER_WMR: ${XRT_BUILD_DRIVER_WMR}")
|
message(STATUS "# DRIVER_WMR: ${XRT_BUILD_DRIVER_WMR}")
|
||||||
|
message(STATUS "# DRIVER_EUROC: ${XRT_BUILD_DRIVER_EUROC}")
|
||||||
message(STATUS "#####----- Config -----#####")
|
message(STATUS "#####----- Config -----#####")
|
||||||
|
|
||||||
if(XRT_FEATURE_SERVICE AND NOT XRT_FEATURE_OPENXR)
|
if(XRT_FEATURE_SERVICE AND NOT XRT_FEATURE_OPENXR)
|
||||||
|
|
|
||||||
|
|
@ -243,6 +243,10 @@ if sdl2.found() and ('auto' in drivers and 'qwerty' not in drivers)
|
||||||
drivers += ['qwerty']
|
drivers += ['qwerty']
|
||||||
endif
|
endif
|
||||||
|
|
||||||
|
if opencv.found() and ('auto' in drivers and 'euroc' not in drivers)
|
||||||
|
drivers += ['euroc']
|
||||||
|
endif
|
||||||
|
|
||||||
if drivers.length() == 0 or drivers == ['auto']
|
if drivers.length() == 0 or drivers == ['auto']
|
||||||
error('You must enable at least one driver.')
|
error('You must enable at least one driver.')
|
||||||
endif
|
endif
|
||||||
|
|
|
||||||
|
|
@ -3,7 +3,7 @@
|
||||||
|
|
||||||
option('drivers',
|
option('drivers',
|
||||||
type: 'array',
|
type: 'array',
|
||||||
choices: ['auto', 'dummy', 'hdk', 'hydra', 'ns', 'ohmd', 'psmv', 'psvr', 'rs', 'v4l2', 'vf', 'depthai', 'vive', 'wmr', 'survive', 'daydream', 'arduino', 'remote', 'handtracking', 'qwerty', 'ulv2'],
|
choices: ['auto', 'dummy', 'hdk', 'hydra', 'ns', 'ohmd', 'psmv', 'psvr', 'rs', 'v4l2', 'vf', 'depthai', 'vive', 'wmr', 'survive', 'daydream', 'arduino', 'remote', 'handtracking', 'qwerty', 'ulv2', 'euroc'],
|
||||||
value: ['auto'],
|
value: ['auto'],
|
||||||
description: 'Set of drivers to build')
|
description: 'Set of drivers to build')
|
||||||
|
|
||||||
|
|
|
||||||
|
|
@ -327,6 +327,18 @@ if(XRT_BUILD_DRIVER_WMR)
|
||||||
list(APPEND ENABLED_HEADSET_DRIVERS wmr)
|
list(APPEND ENABLED_HEADSET_DRIVERS wmr)
|
||||||
endif()
|
endif()
|
||||||
|
|
||||||
|
if(XRT_BUILD_DRIVER_EUROC)
|
||||||
|
set(EUROC_SOURCE_FILES
|
||||||
|
euroc/euroc_driver.cpp
|
||||||
|
euroc/euroc_interface.h
|
||||||
|
)
|
||||||
|
|
||||||
|
add_library(drv_euroc STATIC ${EUROC_SOURCE_FILES})
|
||||||
|
target_link_libraries(drv_euroc PRIVATE xrt-interfaces aux_os aux_util aux_tracking ${OpenCV_LIBRARIES})
|
||||||
|
target_include_directories(drv_euroc PRIVATE ${OpenCV_INCLUDE_DIRS})
|
||||||
|
list(APPEND ENABLED_DRIVERS euroc)
|
||||||
|
endif()
|
||||||
|
|
||||||
if(ENABLED_HEADSET_DRIVERS)
|
if(ENABLED_HEADSET_DRIVERS)
|
||||||
set(ENABLED_DRIVERS ${ENABLED_HEADSET_DRIVERS} ${ENABLED_DRIVERS})
|
set(ENABLED_DRIVERS ${ENABLED_HEADSET_DRIVERS} ${ENABLED_DRIVERS})
|
||||||
list(SORT ENABLED_DRIVERS)
|
list(SORT ENABLED_DRIVERS)
|
||||||
|
|
|
||||||
724
src/xrt/drivers/euroc/euroc_driver.cpp
Normal file
724
src/xrt/drivers/euroc/euroc_driver.cpp
Normal file
|
|
@ -0,0 +1,724 @@
|
||||||
|
// Copyright 2020-2021, Collabora, Ltd.
|
||||||
|
// SPDX-License-Identifier: BSL-1.0
|
||||||
|
/*!
|
||||||
|
* @file
|
||||||
|
* @brief Euroc driver implementation
|
||||||
|
* @author Mateo de Mayo <mateo.demayo@collabora.com>
|
||||||
|
* @ingroup drv_euroc
|
||||||
|
*/
|
||||||
|
|
||||||
|
#include "xrt/xrt_tracking.h"
|
||||||
|
#include "os/os_threading.h"
|
||||||
|
#include "util/u_debug.h"
|
||||||
|
#include "util/u_logging.h"
|
||||||
|
#include "util/u_misc.h"
|
||||||
|
#include "util/u_var.h"
|
||||||
|
#include "tracking/t_frame_cv_mat_wrapper.hpp"
|
||||||
|
#include "math/m_filter_fifo.h"
|
||||||
|
|
||||||
|
#include "euroc_interface.h"
|
||||||
|
|
||||||
|
#include <assert.h>
|
||||||
|
#include <stdio.h>
|
||||||
|
#include <fstream>
|
||||||
|
|
||||||
|
#define EUROC_PLAYER_STR "Euroc Player"
|
||||||
|
#define CLAMP(X, A, B) (MIN(MAX((X), (A)), (B)))
|
||||||
|
|
||||||
|
#define EUROC_TRACE(ep, ...) U_LOG_IFL_T(ep->ll, __VA_ARGS__)
|
||||||
|
#define EUROC_DEBUG(ep, ...) U_LOG_IFL_D(ep->ll, __VA_ARGS__)
|
||||||
|
#define EUROC_INFO(ep, ...) U_LOG_IFL_I(ep->ll, __VA_ARGS__)
|
||||||
|
#define EUROC_WARN(ep, ...) U_LOG_IFL_W(ep->ll, __VA_ARGS__)
|
||||||
|
#define EUROC_ERROR(ep, ...) U_LOG_IFL_E(ep->ll, __VA_ARGS__)
|
||||||
|
#define EUROC_ASSERT(predicate, ...) \
|
||||||
|
do { \
|
||||||
|
bool p = predicate; \
|
||||||
|
if (!p) { \
|
||||||
|
U_LOG(U_LOGGING_ERROR, __VA_ARGS__); \
|
||||||
|
assert(false && "EUROC_ASSERT failed: " #predicate); \
|
||||||
|
exit(EXIT_FAILURE); \
|
||||||
|
} \
|
||||||
|
} while (false);
|
||||||
|
#define EUROC_ASSERT_(predicate) EUROC_ASSERT(predicate, "Assertion failed " #predicate)
|
||||||
|
|
||||||
|
DEBUG_GET_ONCE_LOG_OPTION(euroc_log, "EUROC_LOG", U_LOGGING_WARN)
|
||||||
|
|
||||||
|
|
||||||
|
typedef std::pair<timepoint_ns, std::string> img_sample;
|
||||||
|
typedef std::vector<xrt_imu_sample> imu_samples;
|
||||||
|
typedef std::vector<img_sample> img_samples;
|
||||||
|
|
||||||
|
enum euroc_player_ui_state
|
||||||
|
{
|
||||||
|
UNINITIALIZED = 0,
|
||||||
|
NOT_STREAMING,
|
||||||
|
STREAM_PLAYING,
|
||||||
|
STREAM_PAUSED,
|
||||||
|
STREAM_ENDED
|
||||||
|
};
|
||||||
|
|
||||||
|
/*!
|
||||||
|
* Euroc player is in charge of the playback of a particular dataset.
|
||||||
|
*
|
||||||
|
* @implements xrt_fs
|
||||||
|
* @implements xrt_frame_node
|
||||||
|
* @implements xrt_imu_sink
|
||||||
|
*/
|
||||||
|
struct euroc_player
|
||||||
|
{
|
||||||
|
struct xrt_fs base;
|
||||||
|
struct xrt_frame_node node;
|
||||||
|
|
||||||
|
// Sinks
|
||||||
|
struct xrt_frame_sink left_dbg_sink; //!< Debug Sink for left camera frames
|
||||||
|
struct xrt_frame_sink right_dbg_sink; //!< Debug sink for right camera frames
|
||||||
|
struct xrt_imu_sink imu_dbg_sink; //!< Debug sink for IMU samples
|
||||||
|
struct xrt_frame_sink *left_sink; //!< Downstream sink for left camera frames
|
||||||
|
struct xrt_frame_sink *right_sink; //!< Downstream sink for right camera frames
|
||||||
|
struct xrt_imu_sink *imu_sink; //!< Downstream sink for IMU samples
|
||||||
|
|
||||||
|
struct os_thread_helper play_thread;
|
||||||
|
enum u_logging_level ll;
|
||||||
|
struct xrt_fs_mode mode; //!< The only fs mode the euroc dataset provides
|
||||||
|
bool is_running; //!< Set only at start and stop of frameserver stream
|
||||||
|
|
||||||
|
//! Contains information about the source dataset; set only at start
|
||||||
|
struct
|
||||||
|
{
|
||||||
|
char path[256];
|
||||||
|
bool is_stereo;
|
||||||
|
bool is_colored;
|
||||||
|
uint32_t width;
|
||||||
|
uint32_t height;
|
||||||
|
} dataset;
|
||||||
|
|
||||||
|
//! Next frame number to use, index in `left_imgs` and `right_imgs`.
|
||||||
|
//! Note that this expects that both cameras provide the same amount of frames.
|
||||||
|
//! Furthermore, it is also expected that their timestamps match.
|
||||||
|
uint64_t img_seq;
|
||||||
|
uint64_t imu_seq; //!< Next imu sample number to use, index in `imus`
|
||||||
|
imu_samples *imus; //!< List of all IMU samples read from the dataset
|
||||||
|
img_samples *left_imgs; //!< List of all image names to read from the dataset
|
||||||
|
img_samples *right_imgs; //!< List of all image names to read from the dataset
|
||||||
|
|
||||||
|
// Timestamp correction fields
|
||||||
|
timepoint_ns base_ts; //!< First imu0 timestamp, samples timestamps are relative to this
|
||||||
|
timepoint_ns start_ts; //!< When did the dataset started to be played
|
||||||
|
timepoint_ns offset_ts; //!< Amount of ns to offset start_ns (pauses, skips, etc)
|
||||||
|
|
||||||
|
//! Playback information.
|
||||||
|
//! Prefer to fill it before starting to push frames. Modifying them on
|
||||||
|
//! runtime will work with the debug sinks but probably not elsewhere
|
||||||
|
struct
|
||||||
|
{
|
||||||
|
bool stereo; //!< Whether to stream both left and right sinks or only left
|
||||||
|
bool color; //!< If RGB available but this is false, images will be loaded in grayscale
|
||||||
|
float skip_first_s; //!< Amount of initial seconds of the dataset to skip
|
||||||
|
float scale; //!< Scale of each frame; e.g., 0.5 (half), 1.0 (avoids resize)
|
||||||
|
float speed; //!< Intended reproduction speed, could be slower due to read times
|
||||||
|
bool send_all_imus_first; //!< If enabled all imu samples will be sent before img samples
|
||||||
|
bool paused; //!< Whether to pause the playback
|
||||||
|
} playback;
|
||||||
|
|
||||||
|
// UI related fields
|
||||||
|
enum euroc_player_ui_state ui_state;
|
||||||
|
struct u_var_button start_btn;
|
||||||
|
struct u_var_button pause_btn;
|
||||||
|
char progress_text[128];
|
||||||
|
struct m_ff_vec3_f32 *gyro_ff; //!< Used for displaying IMU data
|
||||||
|
struct m_ff_vec3_f32 *accel_ff; //!< Same as `gyro_ff`
|
||||||
|
};
|
||||||
|
|
||||||
|
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);
|
||||||
|
|
||||||
|
|
||||||
|
// Euroc functionality
|
||||||
|
|
||||||
|
//! Parse and load all IMU samples into `samples`, assumes data.csv is well formed
|
||||||
|
//! If `ep` is not null, will set `ep->base_ts` with the first timestamp read
|
||||||
|
//! All timestamps are set relative to `ep->base_ts`
|
||||||
|
//! If `read_n` > 0, read at most that amount of samples
|
||||||
|
//! Returns whether the appropriate data.csv file could be opened
|
||||||
|
static bool
|
||||||
|
euroc_player_preload_imu_data(struct euroc_player *ep,
|
||||||
|
std::string dataset_path,
|
||||||
|
imu_samples *samples,
|
||||||
|
int64_t read_n = -1)
|
||||||
|
{
|
||||||
|
std::string csv_filename = dataset_path + "/mav0/imu0/data.csv";
|
||||||
|
std::ifstream fin{csv_filename};
|
||||||
|
if (!fin.is_open()) {
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
std::string line;
|
||||||
|
std::getline(fin, line); // Skip header line
|
||||||
|
bool set_base_ts = ep != nullptr;
|
||||||
|
|
||||||
|
while (std::getline(fin, line) && read_n-- != 0) {
|
||||||
|
timepoint_ns timestamp;
|
||||||
|
double v[6];
|
||||||
|
size_t i = 0;
|
||||||
|
size_t j = line.find(',');
|
||||||
|
timestamp = std::stoll(line.substr(i, j));
|
||||||
|
for (size_t k = 0; k < 6; k++) {
|
||||||
|
i = j;
|
||||||
|
j = line.find(',', i + 1);
|
||||||
|
v[k] = std::stod(line.substr(i + 1, j));
|
||||||
|
}
|
||||||
|
|
||||||
|
// Reading the first IMU sample so its timestamp=0, all others are relative to this
|
||||||
|
if (set_base_ts) {
|
||||||
|
ep->base_ts = timestamp;
|
||||||
|
set_base_ts = false;
|
||||||
|
}
|
||||||
|
timestamp = timestamp - ep->base_ts;
|
||||||
|
|
||||||
|
xrt_imu_sample sample{timestamp, v[3], v[4], v[5], v[0], v[1], v[2]};
|
||||||
|
samples->push_back(sample);
|
||||||
|
}
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
//! Parse and load image names and timestamps into `samples`
|
||||||
|
//! All timestamps are set relative to `ep->base_ts`
|
||||||
|
//! If read_n > 0, read at most that amount of samples
|
||||||
|
//! Returns whether the appropriate data.csv file could be opened
|
||||||
|
static bool
|
||||||
|
euroc_player_preload_img_data(
|
||||||
|
timepoint_ns base_ts, std::string dataset_path, img_samples *samples, bool is_left, int64_t read_n = -1)
|
||||||
|
{
|
||||||
|
// Parse image data, assumes data.csv is well formed
|
||||||
|
std::string cam_name = is_left ? "cam0" : "cam1";
|
||||||
|
std::string imgs_path = dataset_path + "/mav0/" + cam_name + "/data";
|
||||||
|
std::string csv_filename = dataset_path + "/mav0/" + cam_name + "/data.csv";
|
||||||
|
std::ifstream fin{csv_filename};
|
||||||
|
if (!fin.is_open()) {
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
std::string line;
|
||||||
|
std::getline(fin, line); // Skip header line
|
||||||
|
while (std::getline(fin, line) && read_n-- != 0) {
|
||||||
|
size_t i = line.find(',');
|
||||||
|
timepoint_ns timestamp = std::stoll(line.substr(0, i)) - base_ts;
|
||||||
|
std::string img_name_tail = line.substr(i + 1);
|
||||||
|
|
||||||
|
// Standard euroc datasets use CRLF line endings, so let's remove the extra '\r'
|
||||||
|
if (img_name_tail.back() == '\r') {
|
||||||
|
img_name_tail.pop_back();
|
||||||
|
}
|
||||||
|
|
||||||
|
std::string img_name = imgs_path + "/" + img_name_tail;
|
||||||
|
img_sample sample{timestamp, img_name};
|
||||||
|
samples->push_back(sample);
|
||||||
|
}
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
static void
|
||||||
|
euroc_player_preload(struct euroc_player *ep)
|
||||||
|
{
|
||||||
|
ep->imus->clear();
|
||||||
|
euroc_player_preload_imu_data(ep, ep->dataset.path, ep->imus);
|
||||||
|
|
||||||
|
ep->left_imgs->clear();
|
||||||
|
euroc_player_preload_img_data(ep->base_ts, ep->dataset.path, ep->left_imgs, true);
|
||||||
|
|
||||||
|
if (ep->dataset.is_stereo) {
|
||||||
|
ep->right_imgs->clear();
|
||||||
|
euroc_player_preload_img_data(ep->base_ts, ep->dataset.path, ep->right_imgs, false);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
//! Skips the first seconds of the dataset as specified by the user
|
||||||
|
static void
|
||||||
|
euroc_player_user_skip(struct euroc_player *ep)
|
||||||
|
{
|
||||||
|
timepoint_ns skip_first_ns = ep->playback.skip_first_s * 1000 * 1000 * 1000;
|
||||||
|
|
||||||
|
while (ep->imus->at(ep->imu_seq).timestamp < skip_first_ns) {
|
||||||
|
ep->imu_seq++;
|
||||||
|
}
|
||||||
|
|
||||||
|
while (ep->left_imgs->at(ep->img_seq).first < skip_first_ns) {
|
||||||
|
ep->img_seq++;
|
||||||
|
}
|
||||||
|
|
||||||
|
ep->offset_ts -= skip_first_ns / ep->playback.speed;
|
||||||
|
}
|
||||||
|
|
||||||
|
//! Determine and fill attributes of the dataset pointed by `path`
|
||||||
|
//! Assertion fails if `path` does not point to an euroc dataset
|
||||||
|
static void
|
||||||
|
euroc_player_fill_dataset_info(struct euroc_player *ep, const char *path)
|
||||||
|
{
|
||||||
|
snprintf(ep->dataset.path, sizeof(ep->dataset.path), "%s", path);
|
||||||
|
img_samples samples;
|
||||||
|
imu_samples _;
|
||||||
|
bool has_right_camera = euroc_player_preload_img_data(0, ep->dataset.path, &samples, false, 0);
|
||||||
|
bool has_left_camera = euroc_player_preload_img_data(0, ep->dataset.path, &samples, true, 1);
|
||||||
|
bool has_imu = euroc_player_preload_imu_data(nullptr, ep->dataset.path, &_, 0);
|
||||||
|
bool is_valid_dataset = has_left_camera && has_imu;
|
||||||
|
EUROC_ASSERT(is_valid_dataset, "Invalid dataset %s", path);
|
||||||
|
|
||||||
|
cv::Mat first_left_img = cv::imread(samples[0].second, cv::IMREAD_ANYCOLOR);
|
||||||
|
ep->dataset.is_stereo = has_right_camera;
|
||||||
|
ep->dataset.is_colored = first_left_img.channels() == 3;
|
||||||
|
ep->dataset.width = first_left_img.cols;
|
||||||
|
ep->dataset.height = first_left_img.rows;
|
||||||
|
EUROC_INFO(ep, "dataset information\n\tpath: %s\n\tis_stereo: %d, is_colored: %d, width: %d, height: %d",
|
||||||
|
ep->dataset.path, ep->dataset.is_stereo, ep->dataset.is_colored, ep->dataset.width,
|
||||||
|
ep->dataset.height);
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
// Playback functionality
|
||||||
|
|
||||||
|
static struct euroc_player *
|
||||||
|
euroc_player(struct xrt_fs *xfs)
|
||||||
|
{
|
||||||
|
return (struct euroc_player *)xfs;
|
||||||
|
}
|
||||||
|
|
||||||
|
//! Wrapper around os_monotonic_get_ns to convert to int64_t and check ranges
|
||||||
|
static timepoint_ns
|
||||||
|
os_monotonic_get_ts()
|
||||||
|
{
|
||||||
|
uint64_t uts = os_monotonic_get_ns();
|
||||||
|
EUROC_ASSERT(uts < INT64_MAX, "Timestamp=%lu was greater than INT64_MAX=%ld", uts, INT64_MAX);
|
||||||
|
int64_t its = uts;
|
||||||
|
return its;
|
||||||
|
}
|
||||||
|
|
||||||
|
//! @returns a timestamp in current time (wrt. ep->start_ts)
|
||||||
|
//! from a relative euroc timestamp (wrt. imu0 first timestamp)
|
||||||
|
static timepoint_ns
|
||||||
|
euroc_player_mapped_ts(struct euroc_player *ep, timepoint_ns relative_ts)
|
||||||
|
{
|
||||||
|
ep->playback.speed = MAX(ep->playback.speed, 1.0 / 256);
|
||||||
|
float speed = ep->playback.speed;
|
||||||
|
timepoint_ns mapped_ts = ep->start_ts + ep->offset_ts + relative_ts / speed;
|
||||||
|
return mapped_ts;
|
||||||
|
}
|
||||||
|
|
||||||
|
static void
|
||||||
|
euroc_player_load_next_frame(struct euroc_player *ep, bool is_left, struct xrt_frame *&xf)
|
||||||
|
{
|
||||||
|
using xrt::auxiliary::tracking::FrameMat;
|
||||||
|
img_sample sample = is_left ? ep->left_imgs->at(ep->img_seq) : ep->right_imgs->at(ep->img_seq);
|
||||||
|
ep->playback.scale = CLAMP(ep->playback.scale, 1.0 / 16, 4);
|
||||||
|
|
||||||
|
// Load will be influenced by these playback options
|
||||||
|
bool use_color = ep->playback.color;
|
||||||
|
float scale = ep->playback.scale;
|
||||||
|
|
||||||
|
// Load image from disk
|
||||||
|
timepoint_ns timestamp = euroc_player_mapped_ts(ep, sample.first);
|
||||||
|
std::string img_name = sample.second;
|
||||||
|
EUROC_TRACE(ep, "%s img t = %ld filename = %s", is_left ? "left" : "right", timestamp, img_name.c_str());
|
||||||
|
cv::ImreadModes read_mode = use_color ? cv::IMREAD_ANYCOLOR : cv::IMREAD_GRAYSCALE;
|
||||||
|
cv::Mat img = cv::imread(img_name, read_mode);
|
||||||
|
|
||||||
|
if (scale != 1.0) {
|
||||||
|
cv::Mat tmp;
|
||||||
|
cv::resize(img, tmp, cv::Size(), scale, scale);
|
||||||
|
img = tmp;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Create xrt_frame, it will be freed by FrameMat destructor
|
||||||
|
EUROC_ASSERT(xf == nullptr || xf->reference.count > 0, "Must be given a valid or nullptr frame ptr");
|
||||||
|
EUROC_ASSERT(timestamp > 0, "Unexpected negative timestamp");
|
||||||
|
FrameMat::Params params{XRT_STEREO_FORMAT_NONE, static_cast<uint64_t>(timestamp)};
|
||||||
|
FrameMat::wrapL8(img, &xf, params);
|
||||||
|
|
||||||
|
// Fields that aren't set by FrameMat
|
||||||
|
xf->owner = &ep->base;
|
||||||
|
xf->source_timestamp = os_monotonic_get_ns(); // Unused
|
||||||
|
xf->source_sequence = ep->img_seq;
|
||||||
|
xf->source_id = ep->base.source_id;
|
||||||
|
}
|
||||||
|
|
||||||
|
static bool
|
||||||
|
euroc_player_is_imu_next(struct euroc_player *ep)
|
||||||
|
{
|
||||||
|
bool prioritize_imus = ep->playback.send_all_imus_first;
|
||||||
|
bool more_imus = ep->imu_seq < ep->imus->size();
|
||||||
|
if (more_imus && prioritize_imus) {
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
bool more_imgs = ep->img_seq < ep->left_imgs->size();
|
||||||
|
timepoint_ns imu_ts = more_imus ? ep->imus->at(ep->imu_seq).timestamp : INT64_MAX;
|
||||||
|
timepoint_ns img_ts = more_imgs ? ep->left_imgs->at(ep->img_seq).first : INT64_MAX;
|
||||||
|
return imu_ts < img_ts;
|
||||||
|
}
|
||||||
|
|
||||||
|
static void
|
||||||
|
euroc_player_push_next_sample(struct euroc_player *ep)
|
||||||
|
{
|
||||||
|
bool stereo = ep->playback.stereo;
|
||||||
|
|
||||||
|
// Push next IMU sample
|
||||||
|
if (euroc_player_is_imu_next(ep)) {
|
||||||
|
xrt_imu_sample sample = ep->imus->at(ep->imu_seq++);
|
||||||
|
sample.timestamp = euroc_player_mapped_ts(ep, sample.timestamp);
|
||||||
|
xrt_sink_push_imu(ep->imu_sink, &sample);
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Push next frame(s)
|
||||||
|
struct xrt_frame *left_xf = NULL, *right_xf = NULL;
|
||||||
|
euroc_player_load_next_frame(ep, true, left_xf);
|
||||||
|
if (stereo) {
|
||||||
|
// TODO: Some SLAM systems expect synced frames, but that's not an
|
||||||
|
// EuRoC requirement. Adapt to work with unsynced datasets too.
|
||||||
|
euroc_player_load_next_frame(ep, false, right_xf);
|
||||||
|
EUROC_ASSERT(left_xf->timestamp == right_xf->timestamp, "Unsynced stereo frames");
|
||||||
|
}
|
||||||
|
ep->img_seq++;
|
||||||
|
|
||||||
|
xrt_sink_push_frame(ep->left_sink, left_xf);
|
||||||
|
if (stereo) {
|
||||||
|
xrt_sink_push_frame(ep->right_sink, left_xf);
|
||||||
|
}
|
||||||
|
|
||||||
|
// We are now done with the frames, unreference them so
|
||||||
|
// they can be freed if all consumers are done with them.
|
||||||
|
xrt_frame_reference(&left_xf, NULL);
|
||||||
|
xrt_frame_reference(&right_xf, NULL);
|
||||||
|
|
||||||
|
snprintf(ep->progress_text, sizeof(ep->progress_text), "Frames %lu/%lu - IMUs %lu/%lu", ep->img_seq,
|
||||||
|
ep->left_imgs->size(), ep->imu_seq, ep->imus->size());
|
||||||
|
|
||||||
|
// Determine how much to sleep until next frame
|
||||||
|
if (ep->img_seq >= ep->left_imgs->size()) {
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
timepoint_ns next_frame_ts = euroc_player_mapped_ts(ep, ep->left_imgs->at(ep->img_seq).first);
|
||||||
|
timepoint_ns now = os_monotonic_get_ts();
|
||||||
|
int32_t frame_delay_ns = MAX(next_frame_ts - now, 0);
|
||||||
|
os_nanosleep(frame_delay_ns);
|
||||||
|
}
|
||||||
|
|
||||||
|
static bool
|
||||||
|
euroc_player_is_paused(struct euroc_player *ep)
|
||||||
|
{
|
||||||
|
if (!ep->playback.paused) {
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
timepoint_ns pre_pause = os_monotonic_get_ts();
|
||||||
|
os_nanosleep(200 * 1000 * 1000);
|
||||||
|
timepoint_ns pos_pause = os_monotonic_get_ts();
|
||||||
|
timepoint_ns pause_length = pos_pause - pre_pause;
|
||||||
|
ep->offset_ts += pause_length;
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
static void *
|
||||||
|
euroc_player_mainloop(void *ptr)
|
||||||
|
{
|
||||||
|
struct xrt_fs *xfs = (struct xrt_fs *)ptr;
|
||||||
|
struct euroc_player *ep = euroc_player(xfs);
|
||||||
|
EUROC_INFO(ep, "Starting euroc playback");
|
||||||
|
|
||||||
|
euroc_player_preload(ep);
|
||||||
|
euroc_player_user_skip(ep);
|
||||||
|
|
||||||
|
ep->start_ts = os_monotonic_get_ts();
|
||||||
|
|
||||||
|
bool more_imus = ep->imu_seq < ep->imus->size();
|
||||||
|
bool more_imgs = ep->img_seq < ep->left_imgs->size();
|
||||||
|
|
||||||
|
while (ep->is_running && (more_imus || more_imgs)) {
|
||||||
|
if (euroc_player_is_paused(ep)) {
|
||||||
|
continue;
|
||||||
|
}
|
||||||
|
|
||||||
|
euroc_player_push_next_sample(ep);
|
||||||
|
|
||||||
|
more_imus = ep->imu_seq < ep->imus->size();
|
||||||
|
more_imgs = ep->img_seq < ep->left_imgs->size();
|
||||||
|
}
|
||||||
|
|
||||||
|
EUROC_INFO(ep, "Euroc dataset playback finished");
|
||||||
|
euroc_player_set_ui_state(ep, STREAM_ENDED);
|
||||||
|
|
||||||
|
return nullptr;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Frame server functionality
|
||||||
|
|
||||||
|
static bool
|
||||||
|
euroc_player_enumerate_modes(struct xrt_fs *xfs, struct xrt_fs_mode **out_modes, uint32_t *out_count)
|
||||||
|
{
|
||||||
|
struct euroc_player *ep = euroc_player(xfs);
|
||||||
|
|
||||||
|
// Should be freed by caller
|
||||||
|
struct xrt_fs_mode *modes = U_TYPED_ARRAY_CALLOC(struct xrt_fs_mode, 1);
|
||||||
|
EUROC_ASSERT(modes != NULL, "Unable to calloc euroc playback modes");
|
||||||
|
|
||||||
|
// At first, it would sound like a good idea to list all possible playback
|
||||||
|
// modes here, however it gets more troublesome than it is worth, and there
|
||||||
|
// doesn't seem to be a good reason to use this feature here. Having said
|
||||||
|
// that, a basic fs mode will be provided, which consists of only the original
|
||||||
|
// properties of the dataset, and ignores the other playback options that can
|
||||||
|
// be tweaked in the UI.
|
||||||
|
modes[0] = ep->mode;
|
||||||
|
|
||||||
|
*out_modes = modes;
|
||||||
|
*out_count = 1;
|
||||||
|
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
static bool
|
||||||
|
euroc_player_configure_capture(struct xrt_fs *xfs, struct xrt_fs_capture_parameters *cp)
|
||||||
|
{
|
||||||
|
// struct euroc_player *ep = euroc_player(xfs);
|
||||||
|
EUROC_ASSERT(false, "Not implemented");
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
static void
|
||||||
|
receive_frame(struct xrt_frame_sink *, struct xrt_frame *)
|
||||||
|
{}
|
||||||
|
|
||||||
|
static void
|
||||||
|
receive_imu_sample(struct xrt_imu_sink *sink, struct xrt_imu_sample *s)
|
||||||
|
{
|
||||||
|
struct euroc_player *ep = container_of(sink, struct euroc_player, imu_dbg_sink);
|
||||||
|
|
||||||
|
// UI log
|
||||||
|
const xrt_vec3 gyro{(float)s->wx, (float)s->wy, (float)s->wz};
|
||||||
|
const xrt_vec3 accel{(float)s->ax, (float)s->ay, (float)s->az};
|
||||||
|
m_ff_vec3_f32_push(ep->gyro_ff, &gyro, s->timestamp);
|
||||||
|
m_ff_vec3_f32_push(ep->accel_ff, &accel, s->timestamp);
|
||||||
|
|
||||||
|
// Trace log
|
||||||
|
U_LOG_IFL_T(debug_get_log_option_euroc_log(), "imu t=%ld ax=%f ay=%f az=%f wx=%f wy=%f wz=%f", s->timestamp,
|
||||||
|
s->ax, s->ay, s->az, s->wx, s->wy, s->wz);
|
||||||
|
}
|
||||||
|
|
||||||
|
//! This is the @ref xrt_fs stream start method, however as the euroc playback
|
||||||
|
//! is heavily customizable, it will be managed through the UI. So this will not
|
||||||
|
//! really start outputting frames but mainly prepare everything to start doing
|
||||||
|
//! so when the user decides
|
||||||
|
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)
|
||||||
|
{
|
||||||
|
struct euroc_player *ep = euroc_player(xfs);
|
||||||
|
ep->is_running = true;
|
||||||
|
|
||||||
|
ep->left_dbg_sink.push_frame = receive_frame;
|
||||||
|
ep->right_dbg_sink.push_frame = receive_frame;
|
||||||
|
ep->imu_dbg_sink.push_imu = receive_imu_sample;
|
||||||
|
|
||||||
|
ep->left_sink = xs != NULL ? xs : &ep->left_dbg_sink;
|
||||||
|
ep->right_sink = &ep->right_dbg_sink; // TODO: Can't be provided by caller
|
||||||
|
ep->imu_sink = &ep->imu_dbg_sink; // TODO: Can't be provided by caller
|
||||||
|
|
||||||
|
if (capture_type == XRT_FS_CAPTURE_TYPE_CALIBRATION) {
|
||||||
|
// On calibration screen don't wait for user input (as we don't have it)
|
||||||
|
euroc_player_start_btn_cb(ep);
|
||||||
|
}
|
||||||
|
|
||||||
|
return ep->is_running;
|
||||||
|
}
|
||||||
|
|
||||||
|
static bool
|
||||||
|
euroc_player_stream_stop(struct xrt_fs *xfs)
|
||||||
|
{
|
||||||
|
struct euroc_player *ep = euroc_player(xfs);
|
||||||
|
ep->is_running = false;
|
||||||
|
|
||||||
|
os_thread_helper_stop(&ep->play_thread);
|
||||||
|
os_thread_helper_destroy(&ep->play_thread);
|
||||||
|
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
static bool
|
||||||
|
euroc_player_is_running(struct xrt_fs *xfs)
|
||||||
|
{
|
||||||
|
struct euroc_player *ep = euroc_player(xfs);
|
||||||
|
return ep->is_running;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Frame node functionality
|
||||||
|
|
||||||
|
static void
|
||||||
|
euroc_player_break_apart(struct xrt_frame_node *node)
|
||||||
|
{
|
||||||
|
struct euroc_player *ep = container_of(node, struct euroc_player, node);
|
||||||
|
euroc_player_stream_stop(&ep->base);
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
static void
|
||||||
|
euroc_player_destroy(struct xrt_frame_node *node)
|
||||||
|
{
|
||||||
|
struct euroc_player *ep = container_of(node, struct euroc_player, node);
|
||||||
|
|
||||||
|
delete ep->imus;
|
||||||
|
delete ep->left_imgs;
|
||||||
|
delete ep->right_imgs;
|
||||||
|
|
||||||
|
u_var_remove_root(ep);
|
||||||
|
|
||||||
|
m_ff_vec3_f32_free(&ep->gyro_ff);
|
||||||
|
m_ff_vec3_f32_free(&ep->accel_ff);
|
||||||
|
|
||||||
|
free(ep);
|
||||||
|
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
// UI functionality
|
||||||
|
|
||||||
|
static void
|
||||||
|
euroc_player_set_ui_state(struct euroc_player *ep, euroc_player_ui_state state)
|
||||||
|
{
|
||||||
|
// -> UNINITIALIZED -> NOT_STREAMING -> STREAM_PLAYING <-> STREAM_PAUSED
|
||||||
|
// └> STREAM_ENDED <┘
|
||||||
|
euroc_player_ui_state prev_state = ep->ui_state;
|
||||||
|
if (state == NOT_STREAMING) {
|
||||||
|
EUROC_ASSERT_(prev_state == UNINITIALIZED);
|
||||||
|
ep->pause_btn.disabled = true;
|
||||||
|
snprintf(ep->progress_text, sizeof(ep->progress_text), "Stream has not started");
|
||||||
|
} else if (state == STREAM_PLAYING) {
|
||||||
|
EUROC_ASSERT_(prev_state == NOT_STREAMING || prev_state == STREAM_PAUSED);
|
||||||
|
ep->start_btn.disabled = true;
|
||||||
|
ep->pause_btn.disabled = false;
|
||||||
|
snprintf(ep->pause_btn.label, sizeof(ep->pause_btn.label), "Pause");
|
||||||
|
} else if (state == STREAM_PAUSED) {
|
||||||
|
EUROC_ASSERT_(prev_state == STREAM_PLAYING);
|
||||||
|
snprintf(ep->pause_btn.label, sizeof(ep->pause_btn.label), "Resume");
|
||||||
|
} else if (state == STREAM_ENDED) {
|
||||||
|
EUROC_ASSERT_(prev_state == STREAM_PLAYING || prev_state == STREAM_PAUSED);
|
||||||
|
ep->pause_btn.disabled = true;
|
||||||
|
} else {
|
||||||
|
EUROC_ASSERT(false, "Unexpected UI state transition from %d to %d", prev_state, state);
|
||||||
|
}
|
||||||
|
ep->ui_state = state;
|
||||||
|
}
|
||||||
|
|
||||||
|
static void
|
||||||
|
euroc_player_start_btn_cb(void *ptr)
|
||||||
|
{
|
||||||
|
struct euroc_player *ep = (struct euroc_player *)ptr;
|
||||||
|
|
||||||
|
int ret = 0;
|
||||||
|
ret |= os_thread_helper_init(&ep->play_thread);
|
||||||
|
ret |= os_thread_helper_start(&ep->play_thread, euroc_player_mainloop, ep);
|
||||||
|
EUROC_ASSERT(ret == 0, "Thread launch failure");
|
||||||
|
|
||||||
|
euroc_player_set_ui_state(ep, STREAM_PLAYING);
|
||||||
|
}
|
||||||
|
|
||||||
|
static void
|
||||||
|
euroc_player_pause_btn_cb(void *ptr)
|
||||||
|
{
|
||||||
|
struct euroc_player *ep = (struct euroc_player *)ptr;
|
||||||
|
ep->playback.paused = !ep->playback.paused;
|
||||||
|
euroc_player_set_ui_state(ep, ep->playback.paused ? STREAM_PAUSED : STREAM_PLAYING);
|
||||||
|
}
|
||||||
|
|
||||||
|
static void
|
||||||
|
euroc_player_setup_gui(struct euroc_player *ep)
|
||||||
|
{
|
||||||
|
// Set fifo queues for display IMU data
|
||||||
|
m_ff_vec3_f32_alloc(&ep->gyro_ff, 1000);
|
||||||
|
m_ff_vec3_f32_alloc(&ep->accel_ff, 1000);
|
||||||
|
|
||||||
|
// Set button callbacks
|
||||||
|
ep->start_btn.cb = euroc_player_start_btn_cb;
|
||||||
|
ep->start_btn.ptr = ep;
|
||||||
|
ep->pause_btn.cb = euroc_player_pause_btn_cb;
|
||||||
|
ep->pause_btn.ptr = ep;
|
||||||
|
euroc_player_set_ui_state(ep, NOT_STREAMING);
|
||||||
|
|
||||||
|
// Add UI wigets
|
||||||
|
u_var_add_root(ep, "Euroc Player", false);
|
||||||
|
u_var_add_ro_text(ep, ep->dataset.path, "Dataset");
|
||||||
|
u_var_add_ro_text(ep, ep->progress_text, "Progress");
|
||||||
|
u_var_add_button(ep, &ep->start_btn, "Start");
|
||||||
|
u_var_add_button(ep, &ep->pause_btn, "Pause");
|
||||||
|
|
||||||
|
u_var_add_gui_header(ep, NULL, "Playback Options");
|
||||||
|
u_var_add_ro_text(ep, "When using a SLAM system, setting these after start is unlikely to work", "Note");
|
||||||
|
u_var_add_bool(ep, &ep->playback.stereo, "Stereo (if available)");
|
||||||
|
u_var_add_bool(ep, &ep->playback.color, "Color (if available)");
|
||||||
|
u_var_add_f32(ep, &ep->playback.skip_first_s, "First seconds to skip (set at start)");
|
||||||
|
u_var_add_f32(ep, &ep->playback.scale, "Scale");
|
||||||
|
u_var_add_f32(ep, &ep->playback.speed, "Speed (set at start)");
|
||||||
|
u_var_add_bool(ep, &ep->playback.send_all_imus_first, "Send all IMU samples now");
|
||||||
|
|
||||||
|
u_var_add_gui_header(ep, NULL, "Streams");
|
||||||
|
u_var_add_ro_ff_vec3_f32(ep, ep->gyro_ff, "Gyroscope");
|
||||||
|
u_var_add_ro_ff_vec3_f32(ep, ep->accel_ff, "Accelerometer");
|
||||||
|
u_var_add_sink(ep, &ep->left_sink, "Left Camera");
|
||||||
|
u_var_add_sink(ep, &ep->right_sink, "Right Camera");
|
||||||
|
}
|
||||||
|
|
||||||
|
// Euroc driver creation
|
||||||
|
|
||||||
|
struct xrt_fs *
|
||||||
|
euroc_player_create(struct xrt_frame_context *xfctx, const char *path)
|
||||||
|
{
|
||||||
|
struct euroc_player *ep = U_TYPED_CALLOC(struct euroc_player);
|
||||||
|
euroc_player_fill_dataset_info(ep, path);
|
||||||
|
ep->mode = xrt_fs_mode{
|
||||||
|
ep->dataset.width,
|
||||||
|
ep->dataset.height,
|
||||||
|
ep->dataset.is_colored ? XRT_FORMAT_R8G8B8 : XRT_FORMAT_R8,
|
||||||
|
// Stereo euroc *is* supported, but we don't expose that through the
|
||||||
|
// xrt_fs interface as it will be managed through two different sinks.
|
||||||
|
XRT_STEREO_FORMAT_NONE,
|
||||||
|
};
|
||||||
|
|
||||||
|
// Using pointers to not mix std::vector with a C-compatible struct
|
||||||
|
ep->imus = new imu_samples{};
|
||||||
|
ep->left_imgs = new img_samples{};
|
||||||
|
ep->right_imgs = new img_samples{};
|
||||||
|
|
||||||
|
ep->playback.stereo = ep->dataset.is_stereo;
|
||||||
|
ep->playback.color = ep->dataset.is_colored;
|
||||||
|
ep->playback.skip_first_s = 0.0;
|
||||||
|
ep->playback.scale = 1.0;
|
||||||
|
ep->playback.speed = 1.0;
|
||||||
|
ep->playback.send_all_imus_first = false;
|
||||||
|
|
||||||
|
ep->ll = debug_get_log_option_euroc_log();
|
||||||
|
euroc_player_setup_gui(ep);
|
||||||
|
|
||||||
|
struct xrt_fs *xfs = &ep->base;
|
||||||
|
xfs->enumerate_modes = euroc_player_enumerate_modes;
|
||||||
|
xfs->configure_capture = euroc_player_configure_capture;
|
||||||
|
xfs->stream_start = euroc_player_stream_start;
|
||||||
|
xfs->stream_stop = euroc_player_stream_stop;
|
||||||
|
xfs->is_running = euroc_player_is_running;
|
||||||
|
|
||||||
|
snprintf(xfs->name, sizeof(xfs->name), EUROC_PLAYER_STR);
|
||||||
|
snprintf(xfs->product, sizeof(xfs->product), EUROC_PLAYER_STR " Product");
|
||||||
|
snprintf(xfs->manufacturer, sizeof(xfs->manufacturer), EUROC_PLAYER_STR " Manufacturer");
|
||||||
|
snprintf(xfs->serial, sizeof(xfs->serial), EUROC_PLAYER_STR " Serial");
|
||||||
|
xfs->source_id = 0xECD0FEED;
|
||||||
|
|
||||||
|
struct xrt_frame_node *xfn = &ep->node;
|
||||||
|
xfn->break_apart = euroc_player_break_apart;
|
||||||
|
xfn->destroy = euroc_player_destroy;
|
||||||
|
|
||||||
|
xrt_frame_context_add(xfctx, &ep->node);
|
||||||
|
|
||||||
|
EUROC_DEBUG(ep, "Euroc player created");
|
||||||
|
|
||||||
|
return xfs;
|
||||||
|
}
|
||||||
45
src/xrt/drivers/euroc/euroc_interface.h
Normal file
45
src/xrt/drivers/euroc/euroc_interface.h
Normal file
|
|
@ -0,0 +1,45 @@
|
||||||
|
// Copyright 2021, Collabora, Ltd.
|
||||||
|
// SPDX-License-Identifier: BSL-1.0
|
||||||
|
/*!
|
||||||
|
* @file
|
||||||
|
* @brief Interface for @ref drv_euroc
|
||||||
|
* @author Mateo de Mayo <mateo.demayo@collabora.com>
|
||||||
|
* @ingroup drv_euroc
|
||||||
|
*/
|
||||||
|
|
||||||
|
#pragma once
|
||||||
|
|
||||||
|
#include "xrt/xrt_frameserver.h"
|
||||||
|
|
||||||
|
#ifdef __cplusplus
|
||||||
|
extern "C" {
|
||||||
|
#endif
|
||||||
|
|
||||||
|
/*!
|
||||||
|
* @defgroup drv_euroc Euroc driver
|
||||||
|
* @ingroup drv
|
||||||
|
*
|
||||||
|
* @brief Provide euroc dataset playback features for SLAM evaluation
|
||||||
|
*
|
||||||
|
* This driver works with any dataset using the EuRoC format.
|
||||||
|
* Original EuRoC datasets and more information about them can be found in
|
||||||
|
* https://projects.asl.ethz.ch/datasets/doku.php?id=kmavvisualinertialdatasets
|
||||||
|
*/
|
||||||
|
|
||||||
|
/*!
|
||||||
|
* Create an euroc player from a path to a dataset.
|
||||||
|
*
|
||||||
|
* @ingroup drv_euroc
|
||||||
|
*/
|
||||||
|
struct xrt_fs *
|
||||||
|
euroc_player_create(struct xrt_frame_context *xfctx, const char *path);
|
||||||
|
|
||||||
|
/*!
|
||||||
|
* @dir drivers/euroc
|
||||||
|
*
|
||||||
|
* @brief @ref drv_euroc files.
|
||||||
|
*/
|
||||||
|
|
||||||
|
#ifdef __cplusplus
|
||||||
|
}
|
||||||
|
#endif
|
||||||
|
|
@ -303,3 +303,14 @@ lib_drv_wmr = static_library(
|
||||||
dependencies: [aux],
|
dependencies: [aux],
|
||||||
build_by_default: 'wmr' in drivers,
|
build_by_default: 'wmr' in drivers,
|
||||||
)
|
)
|
||||||
|
|
||||||
|
lib_drv_euroc = static_library(
|
||||||
|
'drv_euroc',
|
||||||
|
files(
|
||||||
|
'euroc/euroc_driver.cpp',
|
||||||
|
'euroc/euroc_interface.h',
|
||||||
|
),
|
||||||
|
include_directories: [xrt_include],
|
||||||
|
dependencies: [aux, opencv],
|
||||||
|
build_by_default: 'euroc' in drivers,
|
||||||
|
)
|
||||||
|
|
|
||||||
|
|
@ -106,6 +106,9 @@ if(XRT_BUILD_DRIVER_WMR)
|
||||||
target_link_libraries(target_lists PRIVATE drv_wmr)
|
target_link_libraries(target_lists PRIVATE drv_wmr)
|
||||||
endif()
|
endif()
|
||||||
|
|
||||||
|
if(XRT_BUILD_DRIVER_EUROC)
|
||||||
|
target_link_libraries(target_lists PRIVATE drv_euroc)
|
||||||
|
endif()
|
||||||
|
|
||||||
####
|
####
|
||||||
# Instance
|
# Instance
|
||||||
|
|
|
||||||
|
|
@ -89,6 +89,10 @@ if 'ulv2' in drivers
|
||||||
driver_libs += [lib_drv_ulv2]
|
driver_libs += [lib_drv_ulv2]
|
||||||
endif
|
endif
|
||||||
|
|
||||||
|
if 'euroc' in drivers
|
||||||
|
driver_libs += [lib_drv_euroc]
|
||||||
|
endif
|
||||||
|
|
||||||
driver_libs += [lib_drv_multi]
|
driver_libs += [lib_drv_multi]
|
||||||
|
|
||||||
subdir('common')
|
subdir('common')
|
||||||
|
|
|
||||||
Loading…
Reference in a new issue