Monado OpenXR Runtime

Trackers, filters and associated helper code. More...

Collaboration diagram for Tracking:

Files

file  t_calibration.cpp
 Calibration code.
 
file  t_calibration_opencv.hpp
 OpenCV calibration helpers.
 
file  t_camera_models.h
 Simple, untemplated, C, float-only, camera (un)projection functions for various camera models.
 
file  t_convert.cpp
 Code to build conversion tables and convert images.
 
file  t_data_utils.c
 Small data helpers for calibration.
 
file  t_debug_hsv_filter.cpp
 HSV filter debug code.
 
file  t_debug_hsv_picker.cpp
 HSV Picker Debugging code.
 
file  t_debug_hsv_viewer.cpp
 HSV debug viewer code.
 
file  t_documentation.h
 Documentation-only header.
 
file  t_euroc_recorder.cpp
 EuRoC dataset recorder utility.
 
file  t_euroc_recorder.h
 EuRoC dataset recorder utility.
 
file  t_file.cpp
 Handling of files and calibration data.
 
file  t_frame_cv_mat_wrapper.cpp
 Simple xrt_frame wrapper around a cv::Mat.
 
file  t_frame_cv_mat_wrapper.hpp
 Simple xrt_frame wrapper around a cv::Mat.
 
file  t_fusion.hpp
 C++ sensor fusion/filtering code that uses flexkalman.
 
file  t_helper_debug_sink.hpp
 Small helper struct that for debugging views.
 
file  t_hsv_filter.c
 A simple HSV filter.
 
file  t_imu.cpp
 IMU fusion implementation - for inclusion into the single kalman-incuding translation unit.
 
file  t_imu.h
 C interface to basic IMU fusion.
 
file  t_imu_fusion.hpp
 C++ sensor fusion/filtering code that uses flexkalman.
 
file  t_kalman.cpp
 Single compiled file for all kalman filter using source.
 
file  t_openvr_tracker.cpp
 OpenVR tracking source.
 
file  t_openvr_tracker.h
 OpenVR tracking source.
 
file  t_tracker_psmv.cpp
 PS Move tracker code.
 
file  t_tracker_psmv_fusion.cpp
 PS Move tracker code that is expensive to compile.
 
file  t_tracker_psmv_fusion.hpp
 PS Move tracker code.
 
file  t_tracker_psvr.cpp
 PSVR tracker code.
 
file  t_tracker_slam.cpp
 SLAM tracking code.
 
file  t_tracking.h
 Tracking API interface.
 
file  t_vit_loader.c
 Visual-Intertial Tracking consumer helper.
 
file  t_vit_loader.h
 Visual-Intertial Tracking consumer helper.
 
file  u_hand_tracking.c
 Hand Tracking API interface.
 
file  hg_interface.h
 Public interface of Mercury hand tracking.
 

Data Structures

struct  t_camera_calibration_kb4_params_float
 Floating point parameters for T_DISTORTION_FISHEYE_KB4. More...
 
struct  t_camera_calibration_rt8_params_float
 Floating point parameters for T_DISTORTION_OPENCV_RT8, also including metric_radius. More...
 
struct  t_camera_model_params
 Floating point calibration data for a single calibrated camera. More...
 
struct  t_camera_calibration_rt5_params
 Parameters for T_DISTORTION_OPENCV_RADTAN_5. More...
 
struct  t_camera_calibration_rt8_params
 Parameters for T_DISTORTION_OPENCV_RADTAN_8. More...
 
struct  t_camera_calibration_rt14_params
 Parameters for T_DISTORTION_OPENCV_RADTAN_14. More...
 
struct  t_camera_calibration_kb4_params
 Parameters for T_DISTORTION_FISHEYE_KB4. More...
 
struct  t_camera_calibration_wmr_params
 Parameters for T_DISTORTION_WMR. More...
 
struct  t_camera_calibration
 Essential calibration data for a single camera, or single lens/sensor of a stereo camera. More...
 
struct  t_stereo_camera_calibration
 Stereo camera calibration data to be given to trackers. More...
 
struct  t_inertial_calibration
 Parameters for accelerometer and gyroscope calibration. More...
 
struct  t_imu_calibration
 Combined IMU calibration data. More...
 
struct  t_convert_table
 
struct  t_hsv_filter_color
 
struct  t_hsv_filter_params
 Parameters for constructing an HSV filter. More...
 
struct  t_hsv_filter_large_table
 
struct  t_hsv_filter_optimized_table
 
struct  t_slam_camera_calibration
 Extension to camera calibration for SLAM tracking. More...
 
struct  t_slam_imu_calibration
 Extension to IMU calibration for SLAM tracking. More...
 
struct  t_slam_calibration
 Calibration information necessary for SLAM tracking. More...
 
struct  t_slam_tracker_config
 SLAM tracker configuration. More...
 
struct  t_calibration_status
 
struct  t_calibration_params
 
struct  t_vit_bundle
 A bundle of VIT interface functions, used by the tracking interface loader. More...
 
struct  xrt::tracking::hand::mercury::HandTracking
 Main class of Mercury hand tracking. More...
 

Macros

#define XRT_DISTORTION_MAX_DIM   (14)
 Maximum size of rectilinear distortion coefficient array. More...
 
#define T_HSV_SIZE   32
 
#define T_HSV_STEP   (256 / T_HSV_SIZE)
 
#define T_HSV_DEFAULT_PARAMS()
 

Typedefs

typedef struct cJSON cJSON
 

Enumerations

enum  t_camera_distortion_model {
  T_DISTORTION_OPENCV_RADTAN_5 , T_DISTORTION_OPENCV_RADTAN_8 , T_DISTORTION_OPENCV_RADTAN_14 , T_DISTORTION_FISHEYE_KB4 ,
  T_DISTORTION_WMR
}
 The distortion model this camera calibration falls under. More...
 
enum  t_slam_prediction_type {
  SLAM_PRED_NONE = 0 , SLAM_PRED_SP_SO_SA_SL , SLAM_PRED_SP_SO_IA_SL , SLAM_PRED_SP_SO_IA_IL ,
  SLAM_PRED_IP_IO_IA_IL , SLAM_PRED_COUNT
}
 SLAM prediction type. More...
 
enum  t_board_pattern { T_BOARD_CHECKERS , T_BOARD_SB_CHECKERS , T_BOARD_CIRCLES , T_BOARD_ASYMMETRIC_CIRCLES }
 Board pattern type. More...
 

Functions

struct xrt_slam_sinkseuroc_recorder_create (struct xrt_frame_context *xfctx, const char *record_path, int cam_count, bool record_from_start)
 Create SLAM sinks to record samples in EuRoC format. More...
 
static const char * t_stringify_camera_distortion_model (const enum t_camera_distortion_model model)
 Stringifies a t_camera_distortion_model. More...
 
static size_t t_num_params_from_distortion_model (const enum t_camera_distortion_model model)
 Returns the number of parameters needed for this t_camera_distortion_model to be held by an OpenCV Mat and correctly interpreted by OpenCV's (un)projection functions. More...
 
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)
 
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 uint8_t t_hsv_filter_sample (struct t_hsv_filter_optimized_table *t, uint32_t y, uint32_t u, uint32_t v)
 
void t_slam_fill_default_config (struct t_slam_tracker_config *config)
 Fills in a t_slam_tracker_config with default values. More...
 
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)
 
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)
 Create the camera calibration frame sink. More...
 
int t_convert_yuv_or_yuyv_create (struct xrt_frame_sink *next, struct xrt_frame_sink **out_sink)
 
int t_debug_hsv_picker_create (struct xrt_frame_context *xfctx, struct xrt_frame_sink *passthrough, struct xrt_frame_sink **out_sink)
 
int t_debug_hsv_viewer_create (struct xrt_frame_context *xfctx, struct xrt_frame_sink *passthrough, struct xrt_frame_sink **out_sink)
 
int t_debug_hsv_filter_create (struct xrt_frame_context *xfctx, struct xrt_frame_sink *passthrough, struct xrt_frame_sink **out_sink)
 
bool t_vit_bundle_load (struct t_vit_bundle *vit, const char *path)
 Load the tracker. More...
 
void t_vit_bundle_unload (struct t_vit_bundle *vit)
 Unload the tracker. More...
 
struct t_hand_tracking_synct_hand_tracking_sync_mercury_create (struct t_stereo_camera_calibration *calib, struct t_hand_tracking_create_info create_info, const char *models_folder)
 Create a Mercury hand tracking pipeline. More...
 
struct imu_fusionimu_fusion::imu_fusion_create (void)
 Create a struct imu_fusion. More...
 
void imu_fusion::imu_fusion_destroy (struct imu_fusion *fusion)
 Destroy a struct imu_fusion. More...
 
int imu_fusion::imu_fusion_incorporate_gyros (struct imu_fusion *fusion, uint64_t timestamp_ns, struct xrt_vec3 const *ang_vel, struct xrt_vec3 const *ang_vel_variance)
 Predict and correct fusion with a gyroscope reading. More...
 
int imu_fusion::imu_fusion_incorporate_accelerometer (struct imu_fusion *fusion, uint64_t timestamp_ns, struct xrt_vec3 const *accel, struct xrt_vec3 const *accel_variance, struct xrt_vec3 *out_world_accel)
 Predict and correct fusion with an accelerometer reading. More...
 
int imu_fusion::imu_fusion_incorporate_gyros_and_accelerometer (struct imu_fusion *fusion, uint64_t timestamp_ns, struct xrt_vec3 const *ang_vel, struct xrt_vec3 const *ang_vel_variance, struct xrt_vec3 const *accel, struct xrt_vec3 const *accel_variance, struct xrt_vec3 *out_world_accel)
 Predict and correct fusion with a simultaneous accelerometer and gyroscope reading. More...
 
int imu_fusion::imu_fusion_get_prediction (struct imu_fusion const *fusion, uint64_t timestamp_ns, struct xrt_quat *out_quat, struct xrt_vec3 *out_ang_vel)
 Get the predicted state. More...
 
int imu_fusion::imu_fusion_get_prediction_rotation_vec (struct imu_fusion const *fusion, uint64_t timestamp_ns, struct xrt_vec3 *out_rotation_vec)
 Get the predicted state as a rotation vector. More...
 
void t_stereo_camera_calibration::t_stereo_camera_calibration_alloc (struct t_stereo_camera_calibration **out_c, const enum t_camera_distortion_model distortion_model)
 Allocates a new stereo calibration data, unreferences the old data pointed to by out_c. More...
 
static void t_stereo_camera_calibration_reference (struct t_stereo_camera_calibration **dst, struct t_stereo_camera_calibration *src)
 Update the reference counts on a stereo calibration data(s). More...
 
void t_camera_calibration_dump (struct t_camera_calibration *c)
 Small helper function that dumps one camera calibration data to logging. More...
 
void t_stereo_camera_calibration_dump (struct t_stereo_camera_calibration *c)
 Small helper function that dumps the stereo calibration data to logging. More...
 
bool t_stereo_camera_calibration_load_v1 (FILE *calib_file, struct t_stereo_camera_calibration **out_data)
 Load stereo calibration data from a given file in v1 format (binary). More...
 
bool t_stereo_camera_calibration_save_v1 (FILE *calib_file, struct t_stereo_camera_calibration *data)
 Save the given stereo calibration data to the given file in v1 format (binary). More...
 
bool t_stereo_camera_calibration_from_json_v2 (cJSON *json, struct t_stereo_camera_calibration **out_stereo)
 Parse the json object in v2 format into stereo calibration data. More...
 
bool t_stereo_camera_calibration_to_json_v2 (cJSON **out_cjson, struct t_stereo_camera_calibration *data)
 Convert the given stereo calibration data into a json object in v2 format. More...
 
bool t_stereo_camera_calibration_load (const char *calib_path, struct t_stereo_camera_calibration **out_data)
 Load stereo calibration data from a given file path. More...
 
bool t_stereo_camera_calibration_save (const char *calib_path, struct t_stereo_camera_calibration *data)
 Save the given stereo calibration data to the given file path. More...
 
void t_inertial_calibration_dump (struct t_inertial_calibration *c)
 Prints a t_inertial_calibration struct. More...
 
void t_imu_calibration_dump (struct t_imu_calibration *c)
 Small helper function that dumps the imu calibration data to logging. More...
 
int t_hsv_filter::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)
 Construct an HSV filter sink. More...
 
int xrt_tracked_psmv::t_psmv_start (struct xrt_tracked_psmv *xtmv)
 
int xrt_tracked_psmv::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)
 
int xrt_tracked_psvr::t_psvr_start (struct xrt_tracked_psvr *xtvr)
 
int xrt_tracked_psvr::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)
 
int xrt_tracked_slam::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)
 
int xrt_tracked_slam::t_slam_start (struct xrt_tracked_slam *xts)
 
void t_calibration_params::t_calibration_gui_params_default (struct t_calibration_params *p)
 Sets the calibration parameters to the their default values. More...
 

Detailed Description

Trackers, filters and associated helper code.

Coordinate system

Right now there is no specific convention on where a tracking systems coordinate system is centered, and is something we probably need to figure out. Right now the stereo based tracking system used by the PSVR and PSMV tracking system is centered on the camera that OpenCV decided is origin.

To go a bit further on the PSVR/PSMV case. Think about a idealized start up case, the user is wearing the HMD headset and holding two PSMV controllers. The HMD's coordinate system axis are perfectly parallel with the user coordinate with the user's coordinate system. Where -Z is forward. The user holds the controllers with the ball pointing up and the buttons on the back pointing forward. Which if you read the documentation of psmv_device will that the axis of the PSMV are also perfectly aligned with the users coordinate system. So everything "attached" to the user has its coordinate system parallel to the user's.

The camera on the other hand is looking directly at the user, its Z-axis and X-axis is flipped in relation to the user's. So to compare what is sees to what the user sees, everything is rotated 180° around the Y-axis.

Macro Definition Documentation

◆ T_HSV_DEFAULT_PARAMS

#define T_HSV_DEFAULT_PARAMS ( )

#include <auxiliary/tracking/t_tracking.h>

Value:
{ \
{ \
{165, 30, 160, 100}, \
{135, 30, 160, 100}, \
{95, 30, 160, 100}, \
}, \
{128, 80}, \
}

◆ XRT_DISTORTION_MAX_DIM

#define XRT_DISTORTION_MAX_DIM   (14)

#include <auxiliary/tracking/t_tracking.h>

Maximum size of rectilinear distortion coefficient array.

Enumeration Type Documentation

◆ t_board_pattern

#include <auxiliary/tracking/t_tracking.h>

Board pattern type.

Enumerator
T_BOARD_SB_CHECKERS 

Sector based checker board, using cv::findChessboardCornersSB.

◆ t_camera_distortion_model

#include <auxiliary/tracking/t_tracking.h>

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.

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.

Enumerator
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 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:

\[(k_1, k_2, p_1, p_2, k_3)\]

T_DISTORTION_OPENCV_RADTAN_8 

OpenCV's radial-tangential distortion model.

Exactly equivalent to the distortion model from OpenCV's calib3d module, with just the first 8 parameters. Parameters:

\[(k_1, k_2, p_1, p_2, k_3, k_4, k_5, k_6)\]

T_DISTORTION_OPENCV_RADTAN_14 

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:

\[(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)\]

All known factory-calibrated Luxonis cameras use this distortion model, and in all known cases their last 6 parameters are approximately 0.

T_DISTORTION_FISHEYE_KB4 

Juho Kannalla and Sami Sebastian Brandt's fisheye distortion model.

Exactly equivalent to the distortion model from OpenCV's calib3d/fisheye module.

Parameters:

\[(k_1, k_2, k_3, k_4)\]

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_WMR 

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:

\[(k_1, k_2, p_1, p_2, k_3, k_4, k_5, k_6, cod_x, cod_y, rpmax)\]

◆ t_slam_prediction_type

#include <auxiliary/tracking/t_tracking.h>

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 also
xrt_tracked_slam
Enumerator
SLAM_PRED_NONE 

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_IP_IO_IA_IL 

Predicts from a pose that is the last SLAM pose with the IMU samples that came after it integrated on top; velocities from latest IMU sample.

Function Documentation

◆ euroc_recorder_create()

struct xrt_slam_sinks* euroc_recorder_create ( struct xrt_frame_context xfctx,
const char *  record_path,
int  cam_count,
bool  record_from_start 
)

#include <auxiliary/tracking/t_euroc_recorder.h>

Create SLAM sinks to record samples in EuRoC format.

Parameters
xfctxFrame context for the sinks.
record_pathDirectory name to save the dataset or NULL for a default based on the current datetime.
cam_countNumber of cameras to record
record_from_startWhether to start recording immediately on creation.
Returns
struct xrt_slam_sinks* Sinks to push samples to for recording.

References xrt_frame_node::break_apart, euroc_recorder::path, and euroc_recorder::path_prefix.

◆ imu_fusion_create()

struct imu_fusion * imu_fusion_create ( void  )

Create a struct imu_fusion.

◆ imu_fusion_destroy()

void imu_fusion_destroy ( struct imu_fusion fusion)

Destroy a struct imu_fusion.

Should not be called simultaneously with any other imu_fusion function.

Parameters
fusionThe IMU Fusion object

◆ imu_fusion_get_prediction()

int imu_fusion_get_prediction ( struct imu_fusion const *  fusion,
uint64_t  timestamp_ns,
struct xrt_quat out_quat,
struct xrt_vec3 out_ang_vel 
)

Get the predicted state.

Does not advance the internal state clock.

Non-zero return means error.

Parameters
fusionThe IMU Fusion object
timestamp_nsThe timestamp corresponding to the predicted state you want.
out_quatThe quaternion to populate with the predicted orientation.
out_ang_velThe vector to poluate with the predicted angular velocity.

◆ imu_fusion_get_prediction_rotation_vec()

int imu_fusion_get_prediction_rotation_vec ( struct imu_fusion const *  fusion,
uint64_t  timestamp_ns,
struct xrt_vec3 out_rotation_vec 
)

Get the predicted state as a rotation vector.

Does not advance the internal state clock.

This is mostly for debugging: a rotation vector can be easier to visualize or understand intuitively.

Non-zero return means error.

Parameters
fusionThe IMU Fusion object
timestamp_nsThe timestamp corresponding to the predicted state you want.
out_rotation_vecThe vector to poluate with the predicted orientation rotation vector.

◆ imu_fusion_incorporate_accelerometer()

int imu_fusion_incorporate_accelerometer ( struct imu_fusion fusion,
uint64_t  timestamp_ns,
struct xrt_vec3 const *  accel,
struct xrt_vec3 const *  accel_variance,
struct xrt_vec3 out_world_accel 
)

Predict and correct fusion with an accelerometer reading.

If you're receiving accel and gyro data at the same time, call imu_fusion_incorporate_gyros_and_accelerometer() instead.

Should not be called simultaneously with any other imu_fusion function.

Non-zero return means error.

Parameters
fusionThe IMU Fusion object
timestamp_nsThe timestamp corresponding to the information being processed with this call.
accelAccelerometer data (in m/s/s) including the effect of gravity - assumed to be +y when aligned with the world.
accel_varianceThe variance of the accelerometer measurements: part of the characteristics of the IMU being used.
out_world_accelOptional output parameter: will contain the non-gravity acceleration in the world frame.

◆ imu_fusion_incorporate_gyros()

int imu_fusion_incorporate_gyros ( struct imu_fusion fusion,
uint64_t  timestamp_ns,
struct xrt_vec3 const *  ang_vel,
struct xrt_vec3 const *  ang_vel_variance 
)

Predict and correct fusion with a gyroscope reading.

dt should not be zero: If you're receiving accel and gyro data at the same time, call imu_fusion_incorporate_gyros_and_accelerometer() instead.

Should not be called simultaneously with any other imu_fusion function.

Non-zero return means error.

Parameters
fusionThe IMU Fusion object
timestamp_nsThe timestamp corresponding to the information being processed with this call.
ang_velAngular velocity vector from gyroscope: in radians per second.
ang_vel_varianceThe variance of the angular velocity measurements: part of the characteristics of the IMU being used.

◆ imu_fusion_incorporate_gyros_and_accelerometer()

int imu_fusion_incorporate_gyros_and_accelerometer ( struct imu_fusion fusion,
uint64_t  timestamp_ns,
struct xrt_vec3 const *  ang_vel,
struct xrt_vec3 const *  ang_vel_variance,
struct xrt_vec3 const *  accel,
struct xrt_vec3 const *  accel_variance,
struct xrt_vec3 out_world_accel 
)

Predict and correct fusion with a simultaneous accelerometer and gyroscope reading.

Should not be called simultaneously with any other imu_fusion function.

Non-zero return means error.

Parameters
fusionThe IMU Fusion object
timestamp_nsThe timestamp corresponding to the information being processed with this call.
ang_velAngular velocity vector from gyroscope: radians/s
ang_vel_varianceThe variance of the angular velocity measurements: part of the characteristics of the IMU being used.
accelAccelerometer data (in m/s/s) including the effect of gravity - assumed to be +y when aligned with the world.
accel_varianceThe variance of the accelerometer measurements: part of the characteristics of the IMU being used.
out_world_accelOptional output parameter: will contain the non-gravity acceleration in the world frame.

◆ t_calibration_gui_params_default()

void t_calibration_gui_params_default ( struct t_calibration_params p)

Sets the calibration parameters to the their default values.

References t_calibration_params::pattern, t_calibration_params::stereo_sbs, and t_calibration_params::use_fisheye.

◆ t_calibration_stereo_create()

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 
)

#include <auxiliary/tracking/t_tracking.h>

Create the camera calibration frame sink.

Parameters
xfctxContext for frame transport.
paramsParameters to use during calibration. Values copied, pointer not retained.
statusOptional pointer to structure for status information. Pointer retained, and pointed-to struct modified.
guiFrame sink
out_sinkOutput: created frame sink.
See also
xrt_frame_context

◆ t_camera_calibration_dump()

void t_camera_calibration_dump ( struct t_camera_calibration c)
related

Small helper function that dumps one camera calibration data to logging.

References U_LOG_RAW.

Referenced by t_stereo_camera_calibration::t_stereo_camera_calibration_dump().

◆ t_convert_yuv_or_yuyv_create()

int t_convert_yuv_or_yuyv_create ( struct xrt_frame_sink next,
struct xrt_frame_sink **  out_sink 
)

◆ t_debug_hsv_filter_create()

int t_debug_hsv_filter_create ( struct xrt_frame_context xfctx,
struct xrt_frame_sink passthrough,
struct xrt_frame_sink **  out_sink 
)

◆ t_debug_hsv_picker_create()

int t_debug_hsv_picker_create ( struct xrt_frame_context xfctx,
struct xrt_frame_sink passthrough,
struct xrt_frame_sink **  out_sink 
)

◆ t_debug_hsv_viewer_create()

int t_debug_hsv_viewer_create ( struct xrt_frame_context xfctx,
struct xrt_frame_sink passthrough,
struct xrt_frame_sink **  out_sink 
)

◆ t_hand_tracking_sync_mercury_create()

struct t_hand_tracking_sync* t_hand_tracking_sync_mercury_create ( struct t_stereo_camera_calibration calib,
struct t_hand_tracking_create_info  create_info,
const char *  models_folder 
)

#include <tracking/hand/mercury/hg_interface.h>

Create a Mercury hand tracking pipeline.

◆ t_hsv_filter_create()

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 
)

Construct an HSV filter sink.

See also
xrt_frame_context

References xrt_frame_sink::push_frame, and U_TYPED_CALLOC.

◆ t_imu_calibration_dump()

void t_imu_calibration_dump ( struct t_imu_calibration c)
related

Small helper function that dumps the imu calibration data to logging.

◆ t_inertial_calibration_dump()

void t_inertial_calibration_dump ( struct t_inertial_calibration c)
related

Prints a t_inertial_calibration struct.

◆ t_num_params_from_distortion_model()

static size_t t_num_params_from_distortion_model ( const enum t_camera_distortion_model  model)
inlinestatic

#include <auxiliary/tracking/t_tracking.h>

Returns the number of parameters needed for this t_camera_distortion_model to be held by an OpenCV Mat and correctly interpreted by OpenCV's (un)projection functions.

Parameters
modelThe distortion model in question
Returns
The number of distortion coefficients, or 0 if this model cannot be represented inside OpenCV.

References T_DISTORTION_FISHEYE_KB4, T_DISTORTION_OPENCV_RADTAN_14, T_DISTORTION_OPENCV_RADTAN_5, T_DISTORTION_OPENCV_RADTAN_8, T_DISTORTION_WMR, and U_LOG_E.

◆ t_slam_fill_default_config()

◆ t_stereo_camera_calibration_alloc()

void t_stereo_camera_calibration_alloc ( struct t_stereo_camera_calibration **  out_c,
const enum t_camera_distortion_model  distortion_model 
)

◆ t_stereo_camera_calibration_dump()

void t_stereo_camera_calibration_dump ( struct t_stereo_camera_calibration c)
related

Small helper function that dumps the stereo calibration data to logging.

References t_camera_calibration::t_camera_calibration_dump(), U_LOG_RAW, and t_stereo_camera_calibration::view.

◆ t_stereo_camera_calibration_from_json_v2()

bool t_stereo_camera_calibration_from_json_v2 ( cJSON *  json,
struct t_stereo_camera_calibration **  out_stereo 
)
related

Parse the json object in v2 format into stereo calibration data.

◆ t_stereo_camera_calibration_load()

bool t_stereo_camera_calibration_load ( const char *  calib_path,
struct t_stereo_camera_calibration **  out_data 
)
related

Load stereo calibration data from a given file path.

◆ t_stereo_camera_calibration_load_v1()

bool t_stereo_camera_calibration_load_v1 ( FILE *  calib_file,
struct t_stereo_camera_calibration **  out_data 
)
related

Load stereo calibration data from a given file in v1 format (binary).

◆ t_stereo_camera_calibration_reference()

static void t_stereo_camera_calibration_reference ( struct t_stereo_camera_calibration **  dst,
struct t_stereo_camera_calibration src 
)
related

Update the reference counts on a stereo calibration data(s).

Parameters
[in,out]dstPointer to a object reference: if the object reference is non-null will decrement its counter. The reference that dst points to will be set to src.
[in]srcNew object for dst to refer to (may be null). If non-null, will have its refcount increased.

References t_stereo_camera_calibration::reference, xrt_reference::xrt_reference_dec_and_is_zero(), and xrt_reference::xrt_reference_inc().

Referenced by t_stereo_camera_calibration::t_stereo_camera_calibration_alloc().

◆ t_stereo_camera_calibration_save()

bool t_stereo_camera_calibration_save ( const char *  calib_path,
struct t_stereo_camera_calibration data 
)
related

Save the given stereo calibration data to the given file path.

◆ t_stereo_camera_calibration_save_v1()

bool t_stereo_camera_calibration_save_v1 ( FILE *  calib_file,
struct t_stereo_camera_calibration data 
)
related

Save the given stereo calibration data to the given file in v1 format (binary).

◆ t_stereo_camera_calibration_to_json_v2()

bool t_stereo_camera_calibration_to_json_v2 ( cJSON **  out_cjson,
struct t_stereo_camera_calibration data 
)
related

Convert the given stereo calibration data into a json object in v2 format.

◆ t_stringify_camera_distortion_model()

static const char* t_stringify_camera_distortion_model ( const enum t_camera_distortion_model  model)
inlinestatic

#include <auxiliary/tracking/t_tracking.h>

Stringifies a t_camera_distortion_model.

Parameters
modelThe distortion model to be stringified
Returns
The distortion model as a string

References T_DISTORTION_FISHEYE_KB4, T_DISTORTION_OPENCV_RADTAN_14, T_DISTORTION_OPENCV_RADTAN_5, T_DISTORTION_OPENCV_RADTAN_8, T_DISTORTION_WMR, and U_LOG_E.

◆ t_vit_bundle_load()

bool t_vit_bundle_load ( struct t_vit_bundle vit,
const char *  path 
)

#include <auxiliary/tracking/t_vit_loader.h>

Load the tracker.

References U_LOG_E.

◆ t_vit_bundle_unload()

void t_vit_bundle_unload ( struct t_vit_bundle vit)

#include <auxiliary/tracking/t_vit_loader.h>

Unload the tracker.