matrixfurry.com
The open source OpenXR runtime
1// Copyright 2022, Collabora, Ltd.
2// SPDX-License-Identifier: BSL-1.0
3/*!
4 * @file
5 * @brief EuRoC datasets batch evaluation tool
6 * @author Mateo de Mayo <mateo.demayo@collabora.com>
7 */
8
9#include "euroc/euroc_interface.h"
10#include "os/os_threading.h"
11#include "util/u_logging.h"
12#include "xrt/xrt_config_build.h"
13#include "xrt/xrt_config_have.h"
14#include "xrt/xrt_config_drivers.h"
15
16#include <stdio.h>
17
18#define P(...) fprintf(stderr, __VA_ARGS__)
19#define I(...) U_LOG(U_LOGGING_INFO, __VA_ARGS__)
20
21#if defined(XRT_FEATURE_SLAM) && defined(XRT_BUILD_DRIVER_EUROC)
22
23static bool should_exit = false;
24
25static void *
26wait_for_exit_key(void *ptr)
27{
28 getchar();
29 should_exit = true;
30 return NULL;
31}
32#endif
33
34int
35cli_cmd_slambatch(int argc, const char **argv)
36{
37
38#if !defined(XRT_FEATURE_SLAM)
39 P("No SLAM system built.\n");
40 return EXIT_FAILURE;
41#elif !defined(XRT_BUILD_DRIVER_EUROC)
42 P("Euroc driver not built, can't reproduce datasets.\n");
43 return EXIT_FAILURE;
44#else
45 // Do not count "monado-cli" and "slambatch" as args
46 int nof_args = argc - 2;
47 const char **args = &argv[2];
48
49 if (nof_args == 0 || nof_args % 3 != 0) {
50 P("Batch evaluator of SLAM datasets.\n");
51 P("Usage: %s %s [<euroc_path> <slam_config> <output_path>]...\n", argv[0], argv[1]);
52 return EXIT_FAILURE;
53 }
54
55 // Allow pressing enter to quit the program by launching a new thread
56 struct os_thread_helper wfk_thread;
57 os_thread_helper_init(&wfk_thread);
58 os_thread_helper_start(&wfk_thread, wait_for_exit_key, NULL);
59
60 timepoint_ns start_time = os_monotonic_get_ns();
61 int nof_datasets = nof_args / 3;
62 for (int i = 0; i < nof_datasets && !should_exit; i++) {
63 const char *dataset_path = args[i * 3];
64 const char *slam_config = args[i * 3 + 1];
65 const char *output_path = args[i * 3 + 2];
66
67 I("Running dataset %d out of %d", i + 1, nof_datasets);
68 I("Dataset path: %s", dataset_path);
69 I("SLAM config path: %s", slam_config);
70 I("Output path: %s", output_path);
71
72 euroc_run_dataset(dataset_path, slam_config, output_path, &should_exit);
73 }
74 timepoint_ns end_time = os_monotonic_get_ns();
75
76 pthread_cancel(wfk_thread.thread);
77
78 // Destroy also stops the thread.
79 os_thread_helper_destroy(&wfk_thread);
80
81 printf("Done in %.2fs.\n", (double)(end_time - start_time) / U_TIME_1S_IN_NS);
82#endif
83 return EXIT_SUCCESS;
84}