From 1264f385b030bfa3842b946aa6e9aebd61e64064 Mon Sep 17 00:00:00 2001
From: Simon Zeni <simon.zeni@collabora.com>
Date: Thu, 14 Dec 2023 15:59:15 -0500
Subject: [PATCH] external: remove old slam_tracker interface

---
 src/external/CMakeLists.txt                |  10 +-
 src/external/slam_tracker/slam_tracker.hpp | 396 ---------------------
 src/xrt/auxiliary/tracking/CMakeLists.txt  |  13 +-
 3 files changed, 3 insertions(+), 416 deletions(-)
 delete mode 100644 src/external/slam_tracker/slam_tracker.hpp

diff --git a/src/external/CMakeLists.txt b/src/external/CMakeLists.txt
index 1c41fe397..f1d06ebc5 100644
--- a/src/external/CMakeLists.txt
+++ b/src/external/CMakeLists.txt
@@ -80,16 +80,8 @@ target_include_directories(
 	xrt-external-openxr INTERFACE ${CMAKE_CURRENT_SOURCE_DIR}/openxr_includes
 	)
 
-# External SLAM interface
+# External VIT interface
 if(XRT_FEATURE_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 SYSTEM INTERFACE ${SLAM_INCLUDE_DIRS})
-	target_link_libraries(xrt-external-slam INTERFACE ${SLAM_LDFLAGS})
-
 	add_library(xrt-external-vit INTERFACE)
 	target_include_directories(
 		xrt-external-vit INTERFACE ${CMAKE_CURRENT_SOURCE_DIR}/vit_includes
diff --git a/src/external/slam_tracker/slam_tracker.hpp b/src/external/slam_tracker/slam_tracker.hpp
deleted file mode 100644
index ca46a6384..000000000
--- a/src/external/slam_tracker/slam_tracker.hpp
+++ /dev/null
@@ -1,396 +0,0 @@
-// 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 file contains the declaration of the @ref slam_tracker class. This
- * header is intended to appear in both Monado and an external SLAM system. The
- * implementation of `slam_tracker` is provided by the external system.
- * Additional data types are declared for the communication between Monado and
- * the system.
- *
- */
-
-#pragma once
-
-#include <opencv2/core/mat.hpp>
-
-#include <cstdint>
-#include <iostream>
-#include <memory>
-#include <string>
-#include <vector>
-#include <chrono>
-
-namespace xrt::auxiliary::tracking::slam {
-
-// For implementation: same as IMPLEMENTATION_VERSION_*
-// For user: expected IMPLEMENTATION_VERSION_*. Should be checked in runtime.
-constexpr int HEADER_VERSION_MAJOR = 7; //!< API Breakages
-constexpr int HEADER_VERSION_MINOR = 0; //!< Backwards compatible API changes
-constexpr int HEADER_VERSION_PATCH = 0; //!< Backw. comp. .h-implemented changes
-
-// Which header version the external system is implementing.
-extern const int IMPLEMENTATION_VERSION_MAJOR;
-extern const int IMPLEMENTATION_VERSION_MINOR;
-extern const int IMPLEMENTATION_VERSION_PATCH;
-
-enum class pose_ext_type : int;
-
-/*!
- * @brief Standard pose type to communicate Monado with the external SLAM system
- */
-struct pose {
-  std::int64_t timestamp;   //!< In same clock as input samples
-  float px, py, pz;         //!< Position vector
-  float rx, ry, rz, rw = 1; //!< Orientation quaternion
-  std::shared_ptr<struct pose_extension> next = nullptr;
-
-  pose() = default;
-  pose(std::int64_t timestamp,       //
-       float px, float py, float pz, //
-       float rx, float ry, float rz, float rw)
-      : timestamp(timestamp),   //
-        px(px), py(py), pz(pz), //
-        rx(rx), ry(ry), rz(rz), rw(rw) {}
-
-  std::shared_ptr<pose_extension>
-  find_pose_extension(pose_ext_type required_type) const;
-};
-
-struct rect {
-  float x, y, w, h;
-};
-
-/*!
- * @brief IMU Sample type to pass around between programs
- */
-struct imu_sample {
-  std::int64_t timestamp; //!< In nanoseconds
-  double ax, ay, az;      //!< Accel in meters per second squared (m / s^2)
-  double wx, wy, wz;      //!< Gyro in radians per second (rad / s)
-  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;
-  int cam_index;
-  std::vector<rect> masks{}; //!< Masks to ignore
-  img_sample() = default;
-  img_sample(std::int64_t timestamp, const cv::Mat &img, int cam_index)
-      : timestamp(timestamp), img(img), cam_index(cam_index) {}
-};
-
-/*!
- * @brief Parameters for creating the system pipeline.
- */
-struct slam_config {
-  //! Path to a implementation-specific config file. If null, use defaults.
-  std::shared_ptr<std::string> config_file;
-
-  //! Number of cameras to use. Required.
-  int cam_count = -1;
-
-  //! If supported, whether to open the system's UI.
-  bool show_ui = false;
-};
-
-/*!
- * @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 {
-  slam_tracker(const slam_config &config);
-  ~slam_tracker();
-
-  slam_tracker(const slam_tracker &) = delete;
-  slam_tracker &operator=(const slam_tracker &) = delete;
-
-  void initialize();
-  void start();
-  bool is_running();
-  void stop();
-  void finalize();
-
-  /*!
-   * @brief Push an IMU sample into the tracker.
-   *
-   * There must be a single producer thread pushing samples.
-   * Samples must have monotonically increasing timestamps.
-   * The implementation must be non-blocking.
-   * Thus, a separate consumer thread should process the samples.
-   */
-  void push_imu_sample(const imu_sample &sample);
-
-  /*!
-   * @brief Push an image sample into the tracker.
-   *
-   * Same conditions as @ref push_imu_sample apply.
-   * When using N>1 cameras, the N frames must be pushed following cam_id order.
-   * The bundle of N frames must have the same timestamps.
-   */
-  void push_frame(const img_sample &sample);
-
-  /*!
-   * @brief Get the latest tracked pose from the SLAM system.
-   *
-   * There must be a single thread consuming this method.
-   *
-   * @param[out] out_pose Dequeued pose.
-   * @return true If a new pose was dequeued into @p out_pose.
-   * @return false If there was no pose to dequeue.
-   */
-  bool try_dequeue_pose(pose &out_pose);
-
-  //! Asks the SLAM system whether it supports a specific feature.
-  bool supports_feature(int feature_id);
-
-  /*!
-   * @brief Use a special feature of the SLAM tracker.
-   *
-   * This method uses heap allocated objects for passing parameters and
-   * obtaining the results. Use `std::static_pointer_cast` to shared pointers to
-   * the expected types.
-   *
-   * @param feature_id Id of the special feature.
-   * @param params Pointer to the parameter object for this feature.
-   * @param result Pointer to the result produced by the feature call.
-   * @return false if the feature was not supported, true otherwise.
-   */
-  bool use_feature(int feature_id, const std::shared_ptr<void> &params,
-                   std::shared_ptr<void> &result);
-
-private:
-  struct implementation;
-  std::unique_ptr<implementation> impl;
-};
-
-/*
- * Special features
- *
- * A special feature is comprised of an ID, a PARAMS type and a RESULT type. It
- * can be defined using DEFINE_FEATURE. Once defined, the definition should not
- * suffer future changes.
- *
- * One of the main concerns in the features interface is the ability to add new
- * features without being required to update the SLAM systems that are not
- * interested in implementing the feature.
- *
- */
-
-#define DEFINE_FEATURE(NAME, SHORT_NAME, ID, PARAMS_TYPE, RESULT_TYPE)         \
-  using FPARAMS_##SHORT_NAME = PARAMS_TYPE;                                    \
-  using FRESULT_##SHORT_NAME = RESULT_TYPE;                                    \
-  constexpr int FID_##SHORT_NAME = ID;                                         \
-  constexpr int F_##NAME = ID;
-
-/*!
- * Container of parameters for a pinhole camera calibration (fx, fy, cx, cy)
- * with an optional distortion.
- *
- *`distortion_model` and its corresponding `distortion` parameters are not
- * standardized in this struct to facilitate implementation prototyping.
- */
-struct cam_calibration {
-  int cam_index; //!< For multi-camera setups. For stereo 0 ~ left, 1 ~ right.
-  int width, height;                //<! Resolution
-  double frequency;                 //<! Frames per second
-  double fx, fy;                    //<! Focal point
-  double cx, cy;                    //<! Principal point
-  std::string distortion_model;     //!< Models like: none, rt4, rt5, rt8, kb4
-  std::vector<double> distortion{}; //!< Parameters for the distortion_model
-  cv::Matx<double, 4, 4> t_imu_cam; //!< Transformation from IMU to camera
-};
-
-struct inertial_calibration {
-  // Calibration intrinsics to apply to each raw measurement.
-
-  //! This transform will be applied to raw measurements.
-  cv::Matx<double, 3, 3> transform;
-
-  //! Offset to add to raw measurements to; called bias in other contexts.
-  cv::Matx<double, 3, 1> offset;
-
-  // Parameters for the random processes that model this IMU. See section "2.1
-  // Gyro Noise Model" of N. Trawny and S. I. Roumeliotis, "Indirect Kalman
-  // Filter for 3D Attitude Estimation". Analogous for accelerometers.
-  // http://mars.cs.umn.edu/tr/reports/Trawny05b.pdf#page=15
-
-  //! IMU internal bias ~ wiener process with steps N(0, σ²); this field is σ;
-  //! [σ] = U / sqrt(sec³) with U = rad if gyroscope, U = m/s if accelerometer.
-  cv::Matx<double, 3, 1> bias_std;
-
-  //! IMU measurement noise ~ N(0, σ²); this field is σ.
-  //! [σ] = U / sqrt(sec) with U = rad if gyroscope, U = m/s if accelerometer.
-  cv::Matx<double, 3, 1> noise_std;
-
-  inertial_calibration() : transform(cv::Matx<double, 3, 3>::eye()) {}
-};
-
-struct imu_calibration {
-  int imu_index;    //!< For multi-imu setups. Usually just 0.
-  double frequency; //!< Samples per second
-  inertial_calibration accel;
-  inertial_calibration gyro;
-};
-
-/*!
- * Feature ADD_CAMERA_CALIBRATION
- *
- * Use it after constructor but before `start()` to write or overwrite camera
- * calibration data that might come from the system-specific config file.
- */
-DEFINE_FEATURE(ADD_CAMERA_CALIBRATION, ACC, 1, cam_calibration, void)
-
-/*!
- * Feature ADD_IMU_CALIBRATION
- *
- * Use it after constructor but before `start()` to write or overwrite IMU
- * calibration data that might come from the system-specific config file.
- */
-DEFINE_FEATURE(ADD_IMU_CALIBRATION, AIC, 2, imu_calibration, void)
-
-/*!
- * Feature ENABLE_POSE_EXT_TIMING
- *
- * Enable/disable adding internal timestamps to the estimated poses.
- * Returns a vector with names for the timestamps in `pose_ext_timing`.
- */
-DEFINE_FEATURE(ENABLE_POSE_EXT_TIMING, EPET, 3, bool, std::vector<std::string>)
-
-/*!
- * Feature ENABLE_POSE_EXT_FEATURES
- *
- * Enable/disable adding feature information to the estimated poses.
- */
-DEFINE_FEATURE(ENABLE_POSE_EXT_FEATURES, EPEF, 4, bool, void)
-
-/*!
- * Feature RESET_TRACKER_STATE
- *
- * Reset tracker state.
- */
-DEFINE_FEATURE(RESET_TRACKER_STATE, RS, 5, void, void)
-
-/*
- * Pose extensions
- *
- * A pose extension is a struct that gets linked in the `pose.next` field. You
- * first ask if the implementation supports enabling such extension with a
- * `supports_feature()` call with the appropriate `ENABLE_POSE_EXT_*`. Then, it
- * can be enabled with the corresponding `use_feature()` call.
- *
- */
-
-enum class pose_ext_type : int {
-  UNDEFINED = 0,
-  TIMING = 1,
-  FEATURES = 2,
-};
-
-struct pose_extension {
-  pose_ext_type type = pose_ext_type::UNDEFINED;
-  std::shared_ptr<pose_extension> next = nullptr;
-
-  pose_extension(pose_ext_type type) : type(type) {}
-};
-
-inline std::shared_ptr<pose_extension>
-pose::find_pose_extension(pose_ext_type required_type) const {
-  std::shared_ptr<pose_extension> pe = next;
-  while (pe != nullptr && pe->type != required_type) {
-    pe = pe->next;
-  }
-  return pe;
-}
-
-// Timing pose extension
-struct pose_ext_timing_data {
-  //! Internal pipeline stage timestamps of interest when generating the pose.
-  //! In steady clock ns. Must have the same number of elements in the same run.
-  std::vector<std::int64_t> timing{};
-
-  //! Names of each timing stage. Should point to static memory.
-  const std::vector<std::string> *timing_titles = nullptr;
-};
-
-struct pose_ext_timing : pose_extension, pose_ext_timing_data {
-  pose_ext_timing() : pose_extension{pose_ext_type::TIMING} {}
-  pose_ext_timing(const pose_ext_timing_data &petd)
-      : pose_extension{pose_ext_type::TIMING}, pose_ext_timing_data{petd} {}
-};
-
-// Features pose extension
-struct pose_ext_features_data {
-  struct feature {
-    std::int64_t id;
-    float u;
-    float v;
-    float depth;
-  };
-
-  std::vector<std::vector<feature>> features_per_cam{};
-};
-
-struct pose_ext_features : pose_extension, pose_ext_features_data {
-  pose_ext_features() : pose_extension{pose_ext_type::FEATURES} {}
-  pose_ext_features(const pose_ext_features_data &pefd)
-      : pose_extension{pose_ext_type::FEATURES}, pose_ext_features_data{pefd} {}
-};
-
-/*!
- * Utility object to keep track of different stats for a particular timestamp.
- * Stats usually correspond with a particular pose extension.
- */
-struct timestats : pose_ext_timing_data, pose_ext_features_data {
-  using ptr = std::shared_ptr<timestats>;
-
-  std::int64_t ts = -1;
-  bool timing_enabled = false;
-  bool features_enabled = false;
-
-  void addTime(const char *name, int64_t ts = INT64_MIN) {
-    if (!timing_enabled) {
-      return;
-    }
-    if (timing_titles) {
-      std::string expected = timing_titles->at(timing.size());
-      if (expected != name) {
-        std::cout << "Invalid timing stage\n";
-        std::cout << "expected: " << expected;
-        std::cout << ", got: " << name << std::endl;
-        exit(EXIT_FAILURE);
-      }
-    }
-    if (ts == INT64_MIN) {
-      ts = std::chrono::steady_clock::now().time_since_epoch().count();
-    }
-    timing.push_back(ts);
-  }
-
-  void addFeature(size_t cam, const feature &f) {
-    if (!features_enabled) {
-      return;
-    }
-    if (cam >= features_per_cam.size()) {
-      features_per_cam.resize(cam + 1);
-    }
-    features_per_cam.at(cam).push_back(f);
-  }
-};
-
-} // namespace xrt::auxiliary::tracking::slam
diff --git a/src/xrt/auxiliary/tracking/CMakeLists.txt b/src/xrt/auxiliary/tracking/CMakeLists.txt
index 41f87d4a9..4f13ea7c1 100644
--- a/src/xrt/auxiliary/tracking/CMakeLists.txt
+++ b/src/xrt/auxiliary/tracking/CMakeLists.txt
@@ -62,17 +62,8 @@ if(XRT_HAVE_OPENCV)
 endif()
 
 if(XRT_FEATURE_SLAM)
-	target_sources(aux_tracking
-	PRIVATE
-		t_vit_loader.c
-		t_tracker_slam.cpp
-	)
-	target_link_libraries(aux_tracking
-	PRIVATE
-		xrt-external-slam
-		xrt-external-vit
-		${CMAKE_DL_LIBS}
-	)
+	target_sources(aux_tracking PRIVATE t_vit_loader.c t_tracker_slam.cpp)
+	target_link_libraries(aux_tracking PRIVATE xrt-external-vit ${CMAKE_DL_LIBS})
 endif()
 
 if(XRT_HAVE_OPENVR)