diff --git a/src/xrt/targets/cli/CMakeLists.txt b/src/xrt/targets/cli/CMakeLists.txt index 5c8959734..2a6fb3eae 100644 --- a/src/xrt/targets/cli/CMakeLists.txt +++ b/src/xrt/targets/cli/CMakeLists.txt @@ -8,6 +8,7 @@ add_executable( cli cli_cmd_lighthouse.c cli_cmd_probe.c + cli_cmd_slambatch.c cli_cmd_test.c cli_common.h cli_main.c @@ -21,6 +22,6 @@ endif() set_target_properties(cli PROPERTIES OUTPUT_NAME monado-cli PREFIX "") -target_link_libraries(cli PRIVATE aux_os aux_util aux_math target_instance_no_comp) +target_link_libraries(cli PRIVATE aux_os aux_util aux_math target_instance_no_comp drv_includes) install(TARGETS cli RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR}) diff --git a/src/xrt/targets/cli/cli_cmd_slambatch.c b/src/xrt/targets/cli/cli_cmd_slambatch.c new file mode 100644 index 000000000..3dff77748 --- /dev/null +++ b/src/xrt/targets/cli/cli_cmd_slambatch.c @@ -0,0 +1,82 @@ +// Copyright 2022, Collabora, Ltd. +// SPDX-License-Identifier: BSL-1.0 +/*! + * @file + * @brief EuRoC datasets batch evaluation tool + * @author Mateo de Mayo + */ + +#include "euroc/euroc_interface.h" +#include "os/os_threading.h" +#include "util/u_logging.h" +#include "xrt/xrt_config_have.h" +#include "xrt/xrt_config_drivers.h" + +#include + +#define P(...) fprintf(stderr, __VA_ARGS__) +#define I(...) U_LOG(U_LOGGING_INFO, __VA_ARGS__) + +#if defined(XRT_BUILD_DRIVER_EUROC) + +static bool should_exit = false; + +static void * +wait_for_exit_key(void *ptr) +{ + getchar(); + should_exit = true; + return NULL; +} +#endif + +int +cli_cmd_slambatch(int argc, const char **argv) +{ + +#if !defined(XRT_HAVE_SLAM) + P("No SLAM system built.\n"); + return EXIT_FAILURE; +#elif !defined(XRT_BUILD_DRIVER_EUROC) + P("Euroc driver not built, can't reproduce datasets.\n"); + return EXIT_FAILURE; +#else + // Do not count "monado-cli" and "slambatch" as args + int nof_args = argc - 2; + const char **args = &argv[2]; + + if (nof_args == 0 || nof_args % 3 != 0) { + P("Batch evaluator of SLAM datasets.\n"); + P("Usage: %s %s [ ]...\n", argv[0], argv[1]); + return EXIT_FAILURE; + } + + // Allow pressing enter to quit the program by launching a new thread + struct os_thread_helper wfk_thread; + os_thread_helper_init(&wfk_thread); + os_thread_helper_start(&wfk_thread, wait_for_exit_key, NULL); + + timepoint_ns start_time = os_monotonic_get_ns(); + int nof_datasets = nof_args / 3; + for (int i = 0; i < nof_datasets && !should_exit; i++) { + const char *dataset_path = args[i * 3]; + const char *slam_config = args[i * 3 + 1]; + const char *output_path = args[i * 3 + 2]; + + I("Running dataset %d out of %d", i + 1, nof_datasets); + I("Dataset path: %s", dataset_path); + I("SLAM config path: %s", slam_config); + I("Output path: %s", output_path); + + euroc_run_dataset(dataset_path, slam_config, output_path, &should_exit); + } + timepoint_ns end_time = os_monotonic_get_ns(); + + pthread_cancel(wfk_thread.thread); + os_thread_helper_stop(&wfk_thread); + os_thread_helper_destroy(&wfk_thread); + + printf("Done in %.2fs.\n", (double)(end_time - start_time) / U_TIME_1S_IN_NS); +#endif + return EXIT_SUCCESS; +} diff --git a/src/xrt/targets/cli/cli_common.h b/src/xrt/targets/cli/cli_common.h index ea7e8c34f..793d67108 100644 --- a/src/xrt/targets/cli/cli_common.h +++ b/src/xrt/targets/cli/cli_common.h @@ -23,6 +23,9 @@ cli_cmd_lighthouse(int argc, const char **argv); int cli_cmd_probe(int argc, const char **argv); +int +cli_cmd_slambatch(int argc, const char **argv); + int cli_cmd_test(int argc, const char **argv); diff --git a/src/xrt/targets/cli/cli_main.c b/src/xrt/targets/cli/cli_main.c index 0d471e91c..2d5ad52e0 100644 --- a/src/xrt/targets/cli/cli_main.c +++ b/src/xrt/targets/cli/cli_main.c @@ -31,6 +31,7 @@ cli_print_help(int argc, const char **argv) P(" probe - Just probe and then exit.\n"); P(" lighthouse - Control the power of lighthouses [on|off].\n"); P(" calibrate - Calibrate a camera and save config (not implemented yet).\n"); + P(" slambatch - Runs a sequence of EuRoC datasets with the SLAM tracker.\n"); return 1; } @@ -56,5 +57,8 @@ main(int argc, const char **argv) if (strcmp(argv[1], "lighthouse") == 0) { return cli_cmd_lighthouse(argc, argv); } + if (strcmp(argv[1], "slambatch") == 0) { + return cli_cmd_slambatch(argc, argv); + } return cli_print_help(argc, argv); } diff --git a/src/xrt/targets/cli/meson.build b/src/xrt/targets/cli/meson.build index 88e5feb62..a1f60aa42 100644 --- a/src/xrt/targets/cli/meson.build +++ b/src/xrt/targets/cli/meson.build @@ -7,6 +7,7 @@ cli = executable( 'cli_cmd_calibrate.c', 'cli_cmd_lighthouse.c', 'cli_cmd_probe.c', + 'cli_cmd_slambatch.c', 'cli_cmd_test.c', 'cli_common.h', 'cli_main.c', @@ -23,6 +24,7 @@ cli = executable( common_include, drv_include, xrt_include, + cjson_include, ], dependencies: [ libusb,