external: Add slam_tracker.hpp and SLAM build flow

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}")

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

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)

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

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 <mateo.demayo@collabora.com>
+ * @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 <opencv2/core/mat.hpp>
+
+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

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,

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