external/slam: Update slam_tracker interface to support dynamic features

src/external/slam_tracker/slam_tracker.hpp

@@ -6,24 +6,23 @@
  * @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
+ * 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.
  *
- * @todo The interface is preliminary and should be improved to avoid
- * unnecessary copies.
  */
 
 #pragma once
 
 #include <opencv2/core/mat.hpp>
 
+#include <cstdint>
+#include <memory>
+#include <string>
+#include <vector>
+
 namespace xrt::auxiliary::tracking::slam {
 
 /*!
@@ -41,9 +40,9 @@ struct pose {
  * @brief IMU Sample type to pass around between programs
  */
 struct imu_sample {
-  std::int64_t timestamp; // In nanoseconds
-  double ax, ay, az; // In meters per second squared (m / s^2)
-  double wx, wy, wz; // In radians per second (rad / s)
+  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)
@@ -59,7 +58,7 @@ struct img_sample {
   cv::Mat img;
   bool is_left;
   img_sample() = default;
-  img_sample(std::int64_t timestamp, cv::Mat img, bool is_left)
+  img_sample(std::int64_t timestamp, const cv::Mat &img, bool is_left)
       : timestamp(timestamp), img(img), is_left(is_left) {}
 };
 
@@ -79,33 +78,149 @@ struct slam_tracker {
    * them up. Therefore, this constructor receives a path to a
    * implementation-specific configuration file.
    */
-  slam_tracker(std::string config_file);
+  slam_tracker(const std::string &config_file);
   ~slam_tracker();
 
   slam_tracker(const slam_tracker &) = delete;
   slam_tracker &operator=(const slam_tracker &) = delete;
 
+  void initialize();
   void start();
-  void stop();
   bool is_running();
+  void stop();
+  void finalize();
 
-  //! There must be a single producer thread pushing samples.
-  //! Samples must have monotonically increasing timestamps.
-  //! The implementation must be non-blocking.
-  //! A separate consumer thread should process the samples.
-  void push_imu_sample(imu_sample sample);
+  /*!
+   * @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);
 
-  //! Same conditions as `push_imu_sample` apply.
-  //! When using stereo frames, they must be pushed in a left-right order.
-  //! The consecutive left-right pair must have the same timestamps.
-  void push_frame(img_sample sample);
+  /*!
+   * @brief Push an image sample into the tracker.
+   *
+   * Same conditions as @ref push_imu_sample apply.
+   * When using stereo frames, they must be pushed in a left-right order.
+   * The consecutive left-right pair must have the same timestamps.
+   */
+  void push_frame(const img_sample &sample);
 
-  //! There must be a single thread accessing the tracked pose.
-  bool try_dequeue_pose(pose &pose);
+  /*!
+   * @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);
 
- private:
+  //! 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;
-  implementation *impl;
+  std::unique_ptr<implementation> impl;
 };
 
-}  // namespace xrt::auxiliary::tracking::slam
+/*
+ * 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;
+
+struct cam_calibration {
+  enum class cam_model { pinhole, fisheye };
+
+  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
+  cam_model model;
+  std::vector<double> model_params;
+  cv::Matx<double, 4, 4> T_cam_imu; //!< Transformation from camera to imu space
+};
+
+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 apply 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)
+
+} // namespace xrt::auxiliary::tracking::slam

src/xrt/auxiliary/tracking/t_tracker_slam.cpp

@@ -265,6 +265,7 @@ extern "C" void
 t_slam_node_break_apart(struct xrt_frame_node *node)
 {
 	auto &t = *container_of(node, TrackerSlam, node);
+	t.slam->finalize();
 	t.slam->stop();
 	os_thread_helper_stop(&t.oth);
 	SLAM_DEBUG("SLAM tracker dismantled");
@@ -323,6 +324,8 @@ t_slam_create(struct xrt_frame_context *xfctx, struct xrt_tracked_slam **out_xts
 	std::string config_file_string = std::string(config_file);
 	t.slam = new slam_tracker{config_file_string};
 
+	t.slam->initialize();
+
 	t.left_sink.push_frame = t_slam_frame_sink_push_left;
 	t.right_sink.push_frame = t_slam_frame_sink_push_right;
 	t.imu_sink.push_imu = t_slam_imu_sink_push;