monado/src/xrt/auxiliary/tracking/t_tracking.h

// Copyright 2019-2023, Collabora, Ltd.
// SPDX-License-Identifier: BSL-1.0
/*!
 * @file
 * @brief  Tracking API interface.
 * @author Pete Black <pblack@collabora.com>
 * @author Jakob Bornecrantz <jakob@collabora.com>
 * @author Ryan Pavlik <ryan.pavlik@collabora.com>
 * @author Moses Turner <moses@collabora.com>
 * @ingroup aux_tracking
 */

#pragma once

#include "util/u_logging.h"
#include "xrt/xrt_defines.h"
#include "xrt/xrt_frame.h"
#include "util/u_misc.h"

#include <stdio.h>


#ifdef __cplusplus
extern "C" {
#endif

/*!
 * @addtogroup aux_tracking
 * @{
 */


/*
 *
 * Pre-declare
 *
 */

typedef struct cJSON cJSON;
struct xrt_slam_sinks;
struct xrt_tracked_psmv;
struct xrt_tracked_psvr;
struct xrt_tracked_slam;


/*
 *
 * Calibration data.
 *
 */

//! Maximum size of rectilinear distortion coefficient array
#define XRT_DISTORTION_MAX_DIM (14)

/*!
 * @brief The distortion model this camera calibration falls under.
 * @todo Add RiftS's Fisheye62 to this enumerator once we have native support for it in our hand tracking and SLAM.
 * @todo Feel free to add support for T_DISTORTION_OPENCV_RADTAN_4 or T_DISTORTION_OPENCV_RADTAN_12 whenever you have a
 * camera that uses those.
 */
enum t_camera_distortion_model
{
	/*!
	 * OpenCV's radial-tangential distortion model. Exactly equivalent to the distortion model from OpenCV's calib3d
	 * module with just the first five parameters. This may be reinterpreted as RT8 with the last three parameters
	 * zeroed out, which is 100% valid and results in exactly equivalent (un)projections.
	 *
	 * Parameters:
	 *
	 * \f[(k_1, k_2, p_1, p_2, k_3)\f]
	 */
	T_DISTORTION_OPENCV_RADTAN_5,

	/*!
	 * OpenCV's radial-tangential distortion model. Exactly equivalent to the distortion model from OpenCV's calib3d
	 * module, with just the first 8 parameters.
	 * Parameters:
	 *
	 * \f[(k_1, k_2, p_1, p_2, k_3, k_4, k_5, k_6)\f]
	 */
	T_DISTORTION_OPENCV_RADTAN_8,

	/*!
	 * OpenCV's radial-tangential distortion model. Exactly equivalent to the distortion model from OpenCV's calib3d
	 * module, with all 14 parameters.
	 *
	 * In practice this is reinterpreted as RT8 because the last 6 parameters are almost always approximately 0.
	 *
	 * @todo Feel free to implement RT14 (un)projection functions if you have a camera that actually has a tilted
	 * sensor.
	 *
	 * Parameters:
	 *
	 * \f[(k_1, k_2, p_1, p_2, k_3, k_4, k_5, k_6, s_1, s_2, s_3, s_4, \tau_x, \tau_y)\f]
	 *
	 * All known factory-calibrated Luxonis cameras use this distortion model, and in all known cases their last 6
	 * parameters are approximately 0.
	 *
	 */
	T_DISTORTION_OPENCV_RADTAN_14,

	/*!
	 * Juho Kannalla and Sami Sebastian Brandt's fisheye distortion model. Exactly equivalent to the distortion
	 * model from OpenCV's calib3d/fisheye module.
	 *
	 * Parameters:
	 *
	 * \f[(k_1, k_2, k_3, k_4)\f]
	 *
	 * Many cameras use this model. Here's a non-exhaustive list of cameras Monado knows about that fall under this
	 * model:
	 * * Intel T265
	 * * Valve Index
	 */
	T_DISTORTION_FISHEYE_KB4,

	/*!
	 * Windows Mixed Reality headsets' camera model.
	 *
	 * The model is listed as CALIBRATION_LensDistortionModelRational6KT in the WMR json files, which seems to be
	 * equivalent to Azure-Kinect-Sensor-SDK's K4A_CALIBRATION_LENS_DISTORTION_MODEL_RATIONAL_6KT.
	 *
	 * The only difference between this model and RT8 are codx, cody, and the way p1 and p2 are interpreted. In
	 * practice we reinterpret this as RT8 because those values are almost always approximately 0 for WMR headsets.
	 *
	 * Parameters:
	 *
	 * \f[(k_1, k_2, p_1, p_2, k_3, k_4, k_5, k_6, cod_x, cod_y, rpmax)\f]
	 */
	T_DISTORTION_WMR,
};


/*!
 * Stringifies a @ref enum t_camera_distortion_model
 * @param model The distortion model to be stringified
 * @return The distortion model as a string
 */
static inline const char *
t_stringify_camera_distortion_model(const enum t_camera_distortion_model model)
{
	switch (model) {
	case T_DISTORTION_OPENCV_RADTAN_5: return "T_DISTORTION_OPENCV_RADTAN_5"; break;
	case T_DISTORTION_OPENCV_RADTAN_8: return "T_DISTORTION_OPENCV_RADTAN_8"; break;
	case T_DISTORTION_OPENCV_RADTAN_14: return "T_DISTORTION_OPENCV_RADTAN_14"; break;
	case T_DISTORTION_WMR: return "T_DISTORTION_WMR"; break;
	case T_DISTORTION_FISHEYE_KB4: return "T_DISTORTION_FISHEYE_KB4"; break;
	default: U_LOG_E("Invalid distortion_model! %d", model); return "INVALID";
	}
}

/*!
 * Returns the number of parameters needed for this @ref enum t_camera_distortion_model to be held by an OpenCV Mat and
 * correctly interpreted by OpenCV's (un)projection functions.
 *
 * @param model The distortion model in question
 * @return The number of distortion coefficients, or 0 if this model cannot be represented inside OpenCV.
 */
static inline size_t
t_num_params_from_distortion_model(const enum t_camera_distortion_model model)
{
	switch (model) {
	case T_DISTORTION_OPENCV_RADTAN_5: return 5; break;
	case T_DISTORTION_OPENCV_RADTAN_8: return 8; break;
	case T_DISTORTION_OPENCV_RADTAN_14: return 14; break;
	case T_DISTORTION_WMR: return 11; break;
	case T_DISTORTION_FISHEYE_KB4: return 4; break;
	default: U_LOG_E("Invalid distortion_model! %d", model); return 0;
	}
}

/*!
 * Parameters for @ref T_DISTORTION_OPENCV_RADTAN_5
 * @ingroup aux_tracking
 */
struct t_camera_calibration_rt5_params
{
	double k1, k2, p1, p2, k3;
};

/*!
 * Parameters for @ref T_DISTORTION_OPENCV_RADTAN_8
 * @ingroup aux_tracking
 */
struct t_camera_calibration_rt8_params
{
	double k1, k2, p1, p2, k3, k4, k5, k6;
};

/*!
 * Parameters for @ref T_DISTORTION_OPENCV_RADTAN_14
 * @ingroup aux_tracking
 */
struct t_camera_calibration_rt14_params
{
	double k1, k2, p1, p2, k3, k4, k5, k6, s1, s2, s3, s4, tx, ty;
};

/*!
 * Parameters for @ref T_DISTORTION_FISHEYE_KB4
 * @ingroup aux_tracking
 */
struct t_camera_calibration_kb4_params
{
	double k1, k2, k3, k4;
};

/*!
 * Parameters for @ref T_DISTORTION_WMR
 * @ingroup aux_tracking
 */
struct t_camera_calibration_wmr_params
{
	double k1, k2, p1, p2, k3, k4, k5, k6, codx, cody, rpmax;
};

/*!
 * @brief Essential calibration data for a single camera, or single lens/sensor
 * of a stereo camera.
 */
struct t_camera_calibration
{
	//! Source image size
	struct xrt_size image_size_pixels;

	//! Camera intrinsics matrix
	double intrinsics[3][3];

	union {
		struct t_camera_calibration_rt5_params rt5;
		struct t_camera_calibration_rt8_params rt8;
		struct t_camera_calibration_rt14_params rt14;
		struct t_camera_calibration_kb4_params kb4;
		struct t_camera_calibration_wmr_params wmr;
		double distortion_parameters_as_array[XRT_DISTORTION_MAX_DIM];
	};


	//! Distortion model that this camera uses.
	enum t_camera_distortion_model distortion_model;
};

/*!
 * Stereo camera calibration data to be given to trackers.
 */
struct t_stereo_camera_calibration
{
	//! Ref counting
	struct xrt_reference reference;

	//! Calibration of individual views/sensor
	struct t_camera_calibration view[2];

	//! Translation from first to second in the stereo pair.
	double camera_translation[3];
	//! Rotation matrix from first to second in the stereo pair.
	double camera_rotation[3][3];

	//! Essential matrix.
	double camera_essential[3][3];
	//! Fundamental matrix.
	double camera_fundamental[3][3];
};

/*!
 * Allocates a new stereo calibration data, unreferences the old data pointed to by @p out_c.
 *
 * @public @memberof t_stereo_camera_calibration
 */
void
t_stereo_camera_calibration_alloc(struct t_stereo_camera_calibration **out_c,
                                  const enum t_camera_distortion_model distortion_model);

/*!
 * Only to be called by @p t_stereo_camera_calibration_reference.
 *
 * @private @memberof t_stereo_camera_calibration
 */
void
t_stereo_camera_calibration_destroy(struct t_stereo_camera_calibration *c);

/*!
 * Update the reference counts on a stereo calibration data(s).
 *
 * @param[in,out] dst Pointer to a object reference: if the object reference is
 *                non-null will decrement its counter. The reference that
 *                @p dst points to will be set to @p src.
 * @param[in] src New object for @p dst to refer to (may be null).
 *                If non-null, will have its refcount increased.
 *
 * @relates t_stereo_camera_calibration
 */
static inline void
t_stereo_camera_calibration_reference(struct t_stereo_camera_calibration **dst, struct t_stereo_camera_calibration *src)
{
	struct t_stereo_camera_calibration *old_dst = *dst;

	if (old_dst == src) {
		return;
	}

	if (src) {
		xrt_reference_inc(&src->reference);
	}

	*dst = src;

	if (old_dst) {
		if (xrt_reference_dec(&old_dst->reference)) {
			t_stereo_camera_calibration_destroy(old_dst);
		}
	}
}

/*!
 * Small helper function that dumps one camera calibration data to logging.
 *
 * @relates t_camera_calibration
 */
void
t_camera_calibration_dump(struct t_camera_calibration *c);

/*!
 * Small helper function that dumps the stereo calibration data to logging.
 *
 * @relates t_stereo_camera_calibration
 */
void
t_stereo_camera_calibration_dump(struct t_stereo_camera_calibration *c);

/*!
 * Load stereo calibration data from a given file in v1 format (binary).
 *
 * @relates t_stereo_camera_calibration
 */
bool
t_stereo_camera_calibration_load_v1(FILE *calib_file, struct t_stereo_camera_calibration **out_data);

/*!
 * Save the given stereo calibration data to the given file in v1 format (binary).
 *
 * @relates t_stereo_camera_calibration
 */
bool
t_stereo_camera_calibration_save_v1(FILE *calib_file, struct t_stereo_camera_calibration *data);

/*!
 * Parse the json object in v2 format into stereo calibration data.
 *
 * @relates t_stereo_camera_calibration
 */
bool
t_stereo_camera_calibration_from_json_v2(cJSON *json, struct t_stereo_camera_calibration **out_stereo);

/*!
 * Convert the given stereo calibration data into a json object in v2 format.
 *
 * @relates t_stereo_camera_calibration
 */
bool
t_stereo_camera_calibration_to_json_v2(cJSON **out_cjson, struct t_stereo_camera_calibration *data);


/*!
 * Load stereo calibration data from a given file path.
 *
 * @relates t_stereo_camera_calibration
 */
bool
t_stereo_camera_calibration_load(const char *calib_path, struct t_stereo_camera_calibration **out_data);

/*!
 * Save the given stereo calibration data to the given file path.
 *
 * @relates t_stereo_camera_calibration
 */
bool
t_stereo_camera_calibration_save(const char *calib_path, struct t_stereo_camera_calibration *data);


/*
 *
 * IMU calibration data.
 *
 */

/*!
 * @brief Parameters for accelerometer and gyroscope calibration.
 * @see slam_tracker::imu_calibration for a more detailed description and references.
 */
struct t_inertial_calibration
{
	//! Linear transformation for raw measurements alignment and scaling.
	double transform[3][3];

	//! Offset to apply to raw measurements.
	double offset[3];

	//! Modeled sensor bias. @see slam_tracker::imu_calibration.
	double bias_std[3];

	//! Modeled measurement noise. @see slam_tracker::imu_calibration.
	double noise_std[3];
};

/*!
 * @brief Combined IMU calibration data.
 */
struct t_imu_calibration
{
	//! Accelerometer calibration data.
	struct t_inertial_calibration accel;

	//! Gyroscope calibration data.
	struct t_inertial_calibration gyro;
};
/*!
 * Prints a @ref t_inertial_calibration struct
 *
 * @relates t_camera_calibration
 */
void
t_inertial_calibration_dump(struct t_inertial_calibration *c);

/*!
 * Small helper function that dumps the imu calibration data to logging.
 *
 * @relates t_camera_calibration
 */
void
t_imu_calibration_dump(struct t_imu_calibration *c);


/*
 *
 * Conversion functions.
 *
 */

struct t_convert_table
{
	uint8_t v[256][256][256][3]; // nolint(readability-magic-numbers)
};

void
t_convert_fill_table(struct t_convert_table *t);

void
t_convert_make_y8u8v8_to_r8g8b8(struct t_convert_table *t);

void
t_convert_make_y8u8v8_to_h8s8v8(struct t_convert_table *t);

void
t_convert_make_h8s8v8_to_r8g8b8(struct t_convert_table *t);

void
t_convert_in_place_y8u8v8_to_r8g8b8(uint32_t width, uint32_t height, size_t stride, void *data_ptr);

void
t_convert_in_place_y8u8v8_to_h8s8v8(uint32_t width, uint32_t height, size_t stride, void *data_ptr);

void
t_convert_in_place_h8s8v8_to_r8g8b8(uint32_t width, uint32_t height, size_t stride, void *data_ptr);


/*
 *
 * Filter functions.
 *
 */

#define T_HSV_SIZE 32
#define T_HSV_STEP (256 / T_HSV_SIZE)

#define T_HSV_DEFAULT_PARAMS()                                                                                         \
	{                                                                                                              \
		{                                                                                                      \
		    {165, 30, 160, 100},                                                                               \
		    {135, 30, 160, 100},                                                                               \
		    {95, 30, 160, 100},                                                                                \
		},                                                                                                     \
		    {128, 80},                                                                                         \
	}

struct t_hsv_filter_color
{
	uint8_t hue_min;
	uint8_t hue_range;

	uint8_t s_min;

	uint8_t v_min;
};

/*!
 * Parameters for constructing an HSV filter.
 * @relates t_hsv_filter
 */
struct t_hsv_filter_params
{
	struct t_hsv_filter_color color[3];

	struct
	{
		uint8_t s_max;
		uint8_t v_min;
	} white;
};

struct t_hsv_filter_large_table
{
	uint8_t v[256][256][256];
};

struct t_hsv_filter_optimized_table
{
	uint8_t v[T_HSV_SIZE][T_HSV_SIZE][T_HSV_SIZE];
};

void
t_hsv_build_convert_table(struct t_hsv_filter_params *params, struct t_convert_table *t);

void
t_hsv_build_large_table(struct t_hsv_filter_params *params, struct t_hsv_filter_large_table *t);

void
t_hsv_build_optimized_table(struct t_hsv_filter_params *params, struct t_hsv_filter_optimized_table *t);

static inline uint8_t
t_hsv_filter_sample(struct t_hsv_filter_optimized_table *t, uint32_t y, uint32_t u, uint32_t v)
{
	return t->v[y / T_HSV_STEP][u / T_HSV_STEP][v / T_HSV_STEP];
}

/*!
 * Construct an HSV filter sink.
 * @public @memberof t_hsv_filter
 *
 * @see xrt_frame_context
 */
int
t_hsv_filter_create(struct xrt_frame_context *xfctx,
                    struct t_hsv_filter_params *params,
                    struct xrt_frame_sink *sinks[4],
                    struct xrt_frame_sink **out_sink);


/*
 *
 * Tracker code.
 *
 */

/*!
 * @public @memberof xrt_tracked_psmv
 */
int
t_psmv_start(struct xrt_tracked_psmv *xtmv);

/*!
 * @public @memberof xrt_tracked_psmv
 */
int
t_psmv_create(struct xrt_frame_context *xfctx,
              struct xrt_colour_rgb_f32 *rgb,
              struct t_stereo_camera_calibration *data,
              struct xrt_tracked_psmv **out_xtmv,
              struct xrt_frame_sink **out_sink);

/*!
 * @public @memberof xrt_tracked_psvr
 */
int
t_psvr_start(struct xrt_tracked_psvr *xtvr);

/*!
 * @public @memberof xrt_tracked_psvr
 */
int
t_psvr_create(struct xrt_frame_context *xfctx,
              struct t_stereo_camera_calibration *data,
              struct xrt_tracked_psvr **out_xtvr,
              struct xrt_frame_sink **out_sink);



/*!
 * SLAM prediction type. Naming scheme as follows:
 * P: position, O: orientation, A: angular velocity, L: linear velocity
 * S: From SLAM poses (slow, precise), I: From IMU data (fast, noisy)
 *
 * @see xrt_tracked_slam
 */
enum t_slam_prediction_type
{
	SLAM_PRED_NONE = 0,    //!< No prediction, always return the last SLAM tracked pose
	SLAM_PRED_SP_SO_SA_SL, //!< Predicts from last two SLAM poses only
	SLAM_PRED_SP_SO_IA_SL, //!< Predicts from last SLAM pose with angular velocity computed from IMU
	SLAM_PRED_SP_SO_IA_IL, //!< Predicts from last SLAM pose with angular and linear velocity computed from IMU
	SLAM_PRED_COUNT,
};

/*!
 * This struct complements calibration data from @ref
 * t_stereo_camera_calibration and @ref t_imu_calibration
 *
 * @see xrt_tracked_slam
 */
struct t_slam_calib_extras
{
	double imu_frequency; //! IMU samples per second
	struct
	{
		double frequency;                //!< Camera FPS
		struct xrt_matrix_4x4 T_imu_cam; //!< Transform IMU to camera. Column major.
	} cams[2];
};

/*!
 * SLAM tracker configuration.
 *
 * @see xrt_tracked_slam
 */
struct t_slam_tracker_config
{
	enum u_logging_level log_level; //!< SLAM tracking logging level
	const char *slam_config;        //!< Config file path, format is specific to the SLAM implementation in use
	bool slam_ui;                   //!< Whether to open the external UI of the external SLAM system
	bool submit_from_start;         //!< Whether to submit data to the SLAM tracker without user action
	enum t_slam_prediction_type prediction; //!< Which level of prediction to use
	bool write_csvs;                        //!< Whether to enable CSV writers from the start for later analysis
	const char *csv_path;                   //!< Path to write CSVs to
	bool timing_stat;                       //!< Enable timing metric in external system
	bool features_stat;                     //!< Enable feature metric in external system

	// Instead of a slam_config file you can set custom calibration data
	const struct t_stereo_camera_calibration *stereo_calib; //!< Camera calibration data
	const struct t_imu_calibration *imu_calib;              //!< IMU calibration data
	const struct t_slam_calib_extras *extra_calib;          //!< Extra calibration data
};

/*!
 * Fills in a @ref t_slam_tracker_config with default values.
 *
 * @see xrt_tracked_Slam
 */
void
t_slam_fill_default_config(struct t_slam_tracker_config *config);

/*!
 * @public @memberof xrt_tracked_slam
 */
int
t_slam_create(struct xrt_frame_context *xfctx,
              struct t_slam_tracker_config *config,
              struct xrt_tracked_slam **out_xts,
              struct xrt_slam_sinks **out_sink);

/*!
 * @public @memberof xrt_tracked_slam
 */
int
t_slam_start(struct xrt_tracked_slam *xts);

/*
 *
 * Camera calibration
 *
 */

/*!
 * Board pattern type.
 */
enum t_board_pattern
{
	T_BOARD_CHECKERS,
	//! Sector based checker board, using `cv::findChessboardCornersSB`.
	T_BOARD_SB_CHECKERS,
	T_BOARD_CIRCLES,
	T_BOARD_ASYMMETRIC_CIRCLES,
};

struct t_calibration_status
{
	//! Is calibration finished?
	bool finished;
	//! Was the target found this frame?
	bool found;
	//! Number of frames collected
	int num_collected;
	//! Number of moving frames before another capture
	int cooldown;
	//! Number of non-moving frames before capture.
	int waits_remaining;
	//! Stereo calibration data that was produced.
	struct t_stereo_camera_calibration *stereo_data;
};

struct t_calibration_params
{
	//! Should we use fisheye version of the calibration functions.
	bool use_fisheye;
	//! Is the camera a stereo sbs camera, mostly for image loading.
	bool stereo_sbs;
	//! What type of pattern are we using for calibration.
	enum t_board_pattern pattern;

	struct
	{
		int cols;
		int rows;
		float size_meters;

		bool subpixel_enable;
		int subpixel_size;
	} checkers;

	struct
	{
		int cols;
		int rows;
		float size_meters;

		bool marker;
		bool normalize_image;
	} sb_checkers;

	struct
	{
		int cols;
		int rows;
		float distance_meters;
	} circles;

	struct
	{
		int cols;
		int rows;
		float diagonal_distance_meters;
	} asymmetric_circles;

	struct
	{
		bool enabled;
		int num_images;
	} load;

	int num_cooldown_frames;
	int num_wait_for;
	int num_collect_total;
	int num_collect_restart;

	/*!
	 * Should we mirror the RGB image?
	 *
	 * Before text is written out, has no effect on actual image capture.
	 */
	bool mirror_rgb_image;

	bool save_images;
};

/*!
 * Sets the calibration parameters to the their default values.
 * @public @memberof t_calibration_params
 */
void
t_calibration_gui_params_default(struct t_calibration_params *p);

void
t_calibration_gui_params_load_or_default(struct t_calibration_params *p);

void
t_calibration_gui_params_to_json(cJSON **out_json, struct t_calibration_params *p);

void
t_calibration_gui_params_parse_from_json(const cJSON *params, struct t_calibration_params *p);

/*!
 * @brief Create the camera calibration frame sink.
 *
 * @param xfctx Context for frame transport.
 * @param params Parameters to use during calibration. Values copied, pointer
 * not retained.
 * @param status Optional pointer to structure for status information. Pointer
 * retained, and pointed-to struct modified.
 * @param gui Frame sink
 * @param out_sink Output: created frame sink.
 *
 * @see xrt_frame_context
 */
int
t_calibration_stereo_create(struct xrt_frame_context *xfctx,
                            const struct t_calibration_params *params,
                            struct t_calibration_status *status,
                            struct xrt_frame_sink *gui,
                            struct xrt_frame_sink **out_sink);


/*
 *
 * Sink creation functions.
 *
 */

/*!
 * @see xrt_frame_context
 */
int
t_convert_yuv_or_yuyv_create(struct xrt_frame_sink *next, struct xrt_frame_sink **out_sink);



/*!
 * @see xrt_frame_context
 */
int
t_debug_hsv_picker_create(struct xrt_frame_context *xfctx,
                          struct xrt_frame_sink *passthrough,
                          struct xrt_frame_sink **out_sink);

/*!
 * @see xrt_frame_context
 */
int
t_debug_hsv_viewer_create(struct xrt_frame_context *xfctx,
                          struct xrt_frame_sink *passthrough,
                          struct xrt_frame_sink **out_sink);

/*!
 * @see xrt_frame_context
 */
int
t_debug_hsv_filter_create(struct xrt_frame_context *xfctx,
                          struct xrt_frame_sink *passthrough,
                          struct xrt_frame_sink **out_sink);

/*!
 * @}
 */


#ifdef __cplusplus
}
#endif