From 8e8a9c14a9a2cd0f86289b4370a511b9d2ade531 Mon Sep 17 00:00:00 2001 From: Mateo de Mayo Date: Wed, 8 Sep 2021 23:28:08 -0300 Subject: [PATCH] external: Add slam_tracker.hpp and SLAM build flow --- CMakeLists.txt | 18 +++- meson.build | 9 ++ src/external/CMakeLists.txt | 9 ++ src/external/meson.build | 8 ++ src/external/slam_tracker/slam_tracker.hpp | 101 ++++++++++++++++++ src/xrt/include/xrt/meson.build | 7 ++ .../include/xrt/xrt_config_have.h.cmake_in | 2 + 7 files changed, 153 insertions(+), 1 deletion(-) create mode 100644 src/external/slam_tracker/slam_tracker.hpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 6fe88a1ee..6637dde70 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -126,6 +126,19 @@ if(ANDROID) find_library(ANDROID_LOG_LIBRARY log) endif() +# Find a external SLAM implementation +set(EXTERNAL_SLAM_SYSTEMS kimera_vio) +foreach(slam_system IN LISTS EXTERNAL_SLAM_SYSTEMS) + find_package(${slam_system} CONFIG) + if(${slam_system}_FOUND) + set(SLAM ON) + set(SLAM_NAME ${slam_system}) + set(SLAM_LIBRARIES ${${slam_system}_LIBRARIES}) + set(SLAM_INCLUDE_DIRS ${${slam_system}_INCLUDE_DIRS}) + break() + endif() +endforeach() + # This one is named differently because that's what CTest uses option(BUILD_TESTING "Enable building of the test suite?" ON) @@ -191,7 +204,8 @@ cmake_dependent_option(XRT_HAVE_SYSTEM_CJSON "Enable cJSON from system, instead cmake_dependent_option(XRT_HAVE_GST "Enable gstreamer" ON "GST_FOUND" OFF) cmake_dependent_option(XRT_HAVE_PERCETTO "Enable percetto support" ON "PERCETTO_FOUND" OFF) cmake_dependent_option(XRT_HAVE_ONNXRUNTIME "Enable ONNX runtime support" ON "ONNXRUNTIME_FOUND" OFF) - +cmake_dependent_option(XRT_HAVE_SLAM "Enable SLAM tracking support" ON "SLAM" OFF) +cmake_dependent_option(XRT_HAVE_KIMERA_SLAM "Enable Kimera support" ON "kimera_vio_FOUND" OFF) cmake_dependent_option(XRT_BUILD_DRIVER_PSVR "Enable PSVR HMD driver" ON "HIDAPI_FOUND" OFF) cmake_dependent_option(XRT_BUILD_DRIVER_RS "Enable RealSense device driver" ON "realsense2_FOUND" OFF) cmake_dependent_option(XRT_BUILD_DRIVER_VIVE "Enable driver for HTC Vive, Vive Pro, Valve Index, and their controllers" ON "ZLIB_FOUND AND XRT_HAVE_LINUX" OFF) @@ -367,6 +381,8 @@ message(STATUS "# SDL2: ${XRT_HAVE_SDL2}") message(STATUS "# PERCETTO: ${XRT_HAVE_PERCETTO}") message(STATUS "# ONNXRUNTIME: ${XRT_HAVE_ONNXRUNTIME}") message(STATUS "# SYSTEM_CJSON: ${XRT_HAVE_SYSTEM_CJSON}") +message(STATUS "# KIMERA: ${XRT_HAVE_KIMERA_SLAM}") +message(STATUS "# SLAM: ${XRT_HAVE_SLAM}") message(STATUS "#") message(STATUS "# FEATURE_COMPOSITOR_MAIN: ${XRT_FEATURE_COMPOSITOR_MAIN}") message(STATUS "# FEATURE_SERVICE: ${XRT_FEATURE_SERVICE}") diff --git a/meson.build b/meson.build index 0a29fe6d1..bd17c6ded 100644 --- a/meson.build +++ b/meson.build @@ -90,6 +90,15 @@ if not opencv.found() opencv = dependency('opencv4', required: get_option('tracking')) endif +# Find an external SLAM implementation +external_slam_systems = ['kimera_vio'] +foreach slam_system : external_slam_systems + slam = dependency(slam_system, required: false) + if slam.found() + break + endif +endforeach + if get_option('tracking').enabled() or get_option('tracking').auto() build_tracking = opencv.found() endif diff --git a/src/external/CMakeLists.txt b/src/external/CMakeLists.txt index b2d2998a5..c0481893f 100644 --- a/src/external/CMakeLists.txt +++ b/src/external/CMakeLists.txt @@ -46,6 +46,15 @@ endif() add_library(xrt-external-openxr INTERFACE) target_include_directories(xrt-external-openxr INTERFACE ${CMAKE_CURRENT_SOURCE_DIR}/openxr_includes) +# External SLAM tracking +if (SLAM) + add_library(xrt-external-slam STATIC slam_tracker/slam_tracker.hpp) + set_target_properties(xrt-external-slam PROPERTIES LINKER_LANGUAGE CXX) + target_include_directories(xrt-external-slam INTERFACE ${CMAKE_CURRENT_SOURCE_DIR}/slam_tracker) + target_include_directories(xrt-external-slam INTERFACE ${SLAM_INCLUDE_DIRS}) + target_link_libraries(xrt-external-slam INTERFACE ${SLAM_LIBRARIES}) +endif() + # STB add_library(xrt-external-stb INTERFACE) target_include_directories(xrt-external-stb INTERFACE ${CMAKE_CURRENT_SOURCE_DIR}/stb) diff --git a/src/external/meson.build b/src/external/meson.build index 71300ebd4..f71e6efb7 100644 --- a/src/external/meson.build +++ b/src/external/meson.build @@ -10,3 +10,11 @@ imgui_include = include_directories('imgui') openxr_include = include_directories('openxr_includes') catch2_include = include_directories('Catch2') stb_include = include_directories('stb') +slam_tracker_include = include_directories('slam_tracker') + +if slam.found() + external_slam = declare_dependency( + include_directories: [slam_tracker_include], + dependencies: [slam], + ) +endif diff --git a/src/external/slam_tracker/slam_tracker.hpp b/src/external/slam_tracker/slam_tracker.hpp new file mode 100644 index 000000000..57daa0320 --- /dev/null +++ b/src/external/slam_tracker/slam_tracker.hpp @@ -0,0 +1,101 @@ +// Copyright 2021, Collabora, Ltd. +// SPDX-License-Identifier: BSL-1.0 +/*! + * @file + * @brief SLAM tracker class header for usage in Monado. + * @author Mateo de Mayo + * @ingroup aux_tracking + * + * This header file contains the declaration of the @ref slam_tracker class to + * implement a SLAM system for usage in Monado. + * + * A copy of this file is present in both Monado and any SLAM system that + * intends to be used by Monado. The SLAM system should provide the + * `slam_tracker` implementation so that Monado can use it. + * + * This file also declares additional data types to be used between Monado and + * the system. + * + * @todo The interface is preliminary and should be improved to avoid + * unnecessary copies. + */ + +#pragma once + +#include + +namespace xrt::auxiliary::tracking::slam { + +/*! + * @brief Standard pose type to communicate Monado with the external SLAM system + */ +struct pose { + float px, py, pz; + float rx, ry, rz, rw; + pose() = default; + pose(float px, float py, float pz, float rx, float ry, float rz, float rw) + : px(px), py(py), pz(pz), rx(rx), ry(ry), rz(rz), rw(rw) {} +}; + +/*! + * @brief IMU Sample type to pass around between programs + */ +struct imu_sample { + std::int64_t timestamp; + double ax, ay, az; + double wx, wy, wz; + imu_sample() = default; + imu_sample(std::int64_t timestamp, double ax, double ay, double az, double wx, + double wy, double wz) + : timestamp(timestamp), ax(ax), ay(ay), az(az), wx(wx), wy(wy), wz(wz) {} +}; + +/*! + * @brief Image sample type to pass around between programs. It is expected that + * any SLAM system takes OpenCV matrices as input. + */ +struct img_sample { + std::int64_t timestamp; + cv::Mat img; + bool is_left; + img_sample() = default; + img_sample(std::int64_t timestamp, cv::Mat img, bool is_left) + : timestamp(timestamp), img(img), is_left(is_left) {} +}; + +/*! + * @brief slam_tracker serves as an interface between Monado and external SLAM + * systems. + * + * This class uses the pointer-to-implementation pattern, and its implementation + * should be provided by an external SLAM system. + */ +struct slam_tracker { + /*! + * @brief Construct a new slam tracker object + * + * @param config_file SLAM systems parameters tend to be numerous and very + * specific, so they usually use a configuration file as the main way to set + * them up. Therefore, this constructor receives a path to a + * implementation-specific configuration file. + */ + slam_tracker(std::string config_file); + ~slam_tracker(); + + slam_tracker(const slam_tracker &) = delete; + slam_tracker &operator=(const slam_tracker &) = delete; + + void start(); + void stop(); + bool is_running(); + + void push_imu_sample(imu_sample sample); + void push_frame(img_sample sample); + bool try_dequeue_pose(pose &pose); + + private: + struct implementation; + implementation *impl; +}; + +} // namespace xrt::auxiliary::tracking::slam diff --git a/src/xrt/include/xrt/meson.build b/src/xrt/include/xrt/meson.build index b55979e9f..bcafbe95d 100644 --- a/src/xrt/include/xrt/meson.build +++ b/src/xrt/include/xrt/meson.build @@ -104,6 +104,13 @@ if libbsd.found() and not get_option('libbsd').disabled() have_conf.set('XRT_HAVE_LIBBSD', true) endif +if slam.found() + have_conf.set('XRT_HAVE_SLAM', true) + if slam.name() == 'kimera_vio' + have_conf.set('XRT_HAVE_KIMERA_SLAM', true) + endif +endif + xrt_config_have_h = configure_file( output: 'xrt_config_have.h', configuration: have_conf, diff --git a/src/xrt/include/xrt/xrt_config_have.h.cmake_in b/src/xrt/include/xrt/xrt_config_have.h.cmake_in index c4f978162..91b8899a1 100644 --- a/src/xrt/include/xrt/xrt_config_have.h.cmake_in +++ b/src/xrt/include/xrt/xrt_config_have.h.cmake_in @@ -26,3 +26,5 @@ #cmakedefine XRT_HAVE_V4L2 #cmakedefine XRT_HAVE_VULKAN #cmakedefine XRT_HAVE_PERCETTO +#cmakedefine XRT_HAVE_KIMERA_SLAM +#cmakedefine XRT_HAVE_SLAM