d/euroc: Add euroc runner

This commit is contained in:
Mateo de Mayo 2022-04-06 10:48:18 -03:00
parent 4c85e3fbb3
commit 310248546d
7 changed files with 150 additions and 3 deletions

View file

@ -348,7 +348,7 @@ endif()
if(XRT_BUILD_DRIVER_EUROC)
add_library(
drv_euroc STATIC euroc/euroc_player.cpp euroc/euroc_driver.h euroc/euroc_device.c
euroc/euroc_interface.h
euroc/euroc_interface.h euroc/euroc_runner.c
)
target_link_libraries(
drv_euroc PRIVATE xrt-interfaces aux_util aux_tracking ${OpenCV_LIBRARIES}

View file

@ -24,6 +24,7 @@
DEBUG_GET_ONCE_BOOL_OPTION(euroc_hmd, "EUROC_HMD", false)
DEBUG_GET_ONCE_OPTION(euroc_path, "EUROC_PATH", NULL)
DEBUG_GET_ONCE_LOG_OPTION(euroc_log, "EUROC_LOG", U_LOGGING_WARN)
struct xrt_device *
euroc_device_create(struct xrt_prober *xp);

View file

@ -37,8 +37,6 @@
extern "C" {
#endif
DEBUG_GET_ONCE_LOG_OPTION(euroc_log, "EUROC_LOG", U_LOGGING_WARN)
/*!
* @}
*/

View file

@ -101,6 +101,22 @@ euroc_player_create(struct xrt_frame_context *xfctx, const char *path, struct eu
struct xrt_auto_prober *
euroc_create_auto_prober(void);
/*!
* Tracks an euroc dataset with the SLAM tracker.
*
* @param should_exit External exit condition, the run will end if it becomes true
* @param euroc_path Dataset path
* @param slam_config Path to config file for the SLAM system
* @param output_path Path to write resulting tracking data to
*
* @ingroup drv_euroc
*/
void
euroc_run_dataset(const char *euroc_path,
const char *slam_config,
const char *output_path,
const volatile bool *should_exit);
/*!
* @dir drivers/euroc
*

View file

@ -30,6 +30,7 @@
#include <thread>
//! @see euroc_player_playback_config
DEBUG_GET_ONCE_LOG_OPTION(euroc_log, "EUROC_LOG", U_LOGGING_WARN)
DEBUG_GET_ONCE_OPTION(gt_device_name, "EUROC_GT_DEVICE_NAME", nullptr)
DEBUG_GET_ONCE_OPTION(stereo, "EUROC_STEREO", nullptr)
DEBUG_GET_ONCE_OPTION(color, "EUROC_COLOR", nullptr)

View file

@ -0,0 +1,130 @@
// Copyright 2022, Collabora, Ltd.
// SPDX-License-Identifier: BSL-1.0
/*!
* @file
* @brief Play EuRoC datasets and track them with the SLAM tracker.
* @author Mateo de Mayo <mateo.demayo@collabora.com>
* @ingroup drv_euroc
*/
#include "euroc_driver.h"
#include "os/os_threading.h"
#include "os/os_time.h"
#include "tracking/t_tracking.h"
#include "util/u_logging.h"
#include "util/u_misc.h"
#include "xrt/xrt_config_have.h"
#include "xrt/xrt_defines.h"
#include "xrt/xrt_frame.h"
#include "xrt/xrt_frameserver.h"
#include "xrt/xrt_tracking.h"
#if !defined(XRT_HAVE_SLAM)
void
euroc_run_dataset(const char *euroc_path,
const char *slam_config,
const char *output_path,
const volatile bool *should_exit)
{}
#else
static struct euroc_player_config *
make_euroc_player_config(const char *euroc_path)
{
struct euroc_player_config *ep_config = U_TYPED_CALLOC(struct euroc_player_config);
euroc_player_fill_default_config_for(ep_config, euroc_path);
// Override config to be friendlier for CLI runs unless they were explicitly provided
if (getenv("EUROC_LOG") == NULL) {
ep_config->log_level = U_LOGGING_INFO;
}
if (getenv("EUROC_PLAY_FROM_START") == NULL) {
ep_config->playback.play_from_start = true;
}
if (getenv("EUROC_PRINT_PROGRESS") == NULL) {
ep_config->playback.print_progress = true;
}
if (getenv("EUROC_USE_SOURCE_TS") == NULL) {
ep_config->playback.use_source_ts = true;
}
if (getenv("EUROC_MAX_SPEED") == NULL) {
ep_config->playback.max_speed = true;
}
return ep_config;
}
static struct t_slam_tracker_config *
make_slam_tracker_config(const char *slam_config, const char *output_path)
{
struct t_slam_tracker_config *st_config = U_TYPED_CALLOC(struct t_slam_tracker_config);
t_slam_fill_default_config(st_config);
// Override config to be friendlier for CLI runs unless they were explicitly provided
if (getenv("SLAM_LOG") == NULL) {
st_config->log_level = U_LOGGING_INFO;
}
if (getenv("SLAM_SUBMIT_FROM_START") == NULL) {
st_config->submit_from_start = true;
}
if (getenv("SLAM_PREDICTION_TYPE") == NULL) {
st_config->prediction = SLAM_PRED_NONE;
}
if (getenv("SLAM_WRITE_CSVS") == NULL) {
st_config->write_csvs = true;
}
st_config->slam_config = slam_config;
st_config->csv_path = output_path;
return st_config;
}
void
euroc_run_dataset(const char *euroc_path,
const char *slam_config,
const char *output_path,
const volatile bool *should_exit)
{
struct euroc_player_config *ep_config = make_euroc_player_config(euroc_path);
struct t_slam_tracker_config *st_config = make_slam_tracker_config(slam_config, output_path);
// Frame context that will manage SLAM tracker and euroc player lifetimes
struct xrt_frame_context xfctx = {0};
// Start SLAM tracker
struct xrt_tracked_slam *xts = NULL;
struct xrt_slam_sinks *sinks = NULL;
int ret = t_slam_create(&xfctx, st_config, &xts, &sinks);
EUROC_ASSERT(ret == 0, "Failed to create slam tracker");
t_slam_start(xts);
// Stream euroc player into the tracker
struct xrt_fs *xfs = euroc_player_create(&xfctx, euroc_path, ep_config);
xrt_fs_slam_stream_start(xfs, sinks);
// Let's loop until both the player and the tracker finish
// Last two tracked poses, if they are the same we assume tracking stopped
struct xrt_space_relation a = {0};
struct xrt_space_relation b = {0};
b.pose.orientation.w = 42; // Make b different from a
bool tracking = true;
bool streaming = xrt_fs_is_running(xfs);
while ((streaming || tracking) && !*should_exit) {
os_nanosleep(0.2 * U_TIME_1S_IN_NS);
a = b;
xrt_tracked_slam_get_tracked_pose(xts, os_monotonic_get_ns(), &b);
tracking = memcmp(&a, &b, sizeof(struct xrt_space_relation)) != 0;
streaming = xrt_fs_is_running(xfs);
}
xrt_frame_context_destroy_nodes(&xfctx);
free(st_config);
free(ep_config);
}
#endif

View file

@ -340,6 +340,7 @@ lib_drv_euroc = static_library(
'euroc/euroc_driver.h',
'euroc/euroc_device.c',
'euroc/euroc_interface.h',
'euroc/euroc_runner.c',
),
include_directories: [xrt_include],
dependencies: [aux, opencv],