Monado OpenXR Runtime
t_tracking.h
Go to the documentation of this file.
1// Copyright 2019-2023, Collabora, Ltd.
2// SPDX-License-Identifier: BSL-1.0
3/*!
4 * @file
5 * @brief Tracking API interface.
6 * @author Pete Black <pblack@collabora.com>
7 * @author Jakob Bornecrantz <jakob@collabora.com>
8 * @author Rylie Pavlik <rylie.pavlik@collabora.com>
9 * @author Moshi Turner <moshiturner@protonmail.com>
10 * @ingroup aux_tracking
11 */
12
13#pragma once
14
15#include "util/u_logging.h"
16#include "xrt/xrt_defines.h"
17#include "xrt/xrt_frame.h"
18#include "xrt/xrt_tracking.h"
19#include "util/u_misc.h"
20
21#include <stdio.h>
22
23
24#ifdef __cplusplus
25extern "C" {
26#endif
27
28/*!
29 * @addtogroup aux_tracking
30 * @{
31 */
32
33
34/*
35 *
36 * Pre-declare
37 *
38 */
39
40typedef struct cJSON cJSON;
41struct xrt_slam_sinks;
42struct xrt_tracked_psmv;
43struct xrt_tracked_psvr;
44struct xrt_tracked_slam;
45
46
47/*
48 *
49 * Calibration data.
50 *
51 */
52
53//! Maximum size of rectilinear distortion coefficient array
54#define XRT_DISTORTION_MAX_DIM (14)
55
56/*!
57 * @brief The distortion model this camera calibration falls under.
58 * @todo Add RiftS's Fisheye62 to this enumerator once we have native support for it in our hand tracking and SLAM.
59 * @todo Feel free to add support for T_DISTORTION_OPENCV_RADTAN_4 or T_DISTORTION_OPENCV_RADTAN_12 whenever you have a
60 * camera that uses those.
61 */
63{
64 /*!
65 * OpenCV's radial-tangential distortion model. Exactly equivalent to the distortion model from OpenCV's calib3d
66 * module with just the first five parameters. This may be reinterpreted as RT8 with the last three parameters
67 * zeroed out, which is 100% valid and results in exactly equivalent (un)projections.
68 *
69 * Parameters:
70 *
71 * \f[(k_1, k_2, p_1, p_2, k_3)\f]
72 */
74
75 /*!
76 * OpenCV's radial-tangential distortion model. Exactly equivalent to the distortion model from OpenCV's calib3d
77 * module, with just the first 8 parameters.
78 * Parameters:
79 *
80 * \f[(k_1, k_2, p_1, p_2, k_3, k_4, k_5, k_6)\f]
81 */
83
84 /*!
85 * OpenCV's radial-tangential distortion model. Exactly equivalent to the distortion model from OpenCV's calib3d
86 * module, with all 14 parameters.
87 *
88 * In practice this is reinterpreted as RT8 because the last 6 parameters are almost always approximately 0.
89 *
90 * @todo Feel free to implement RT14 (un)projection functions if you have a camera that actually has a tilted
91 * sensor.
92 *
93 * Parameters:
94 *
95 * \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]
96 *
97 * All known factory-calibrated Luxonis cameras use this distortion model, and in all known cases their last 6
98 * parameters are approximately 0.
99 *
100 */
102
103 /*!
104 * Juho Kannalla and Sami Sebastian Brandt's fisheye distortion model. Exactly equivalent to the distortion
105 * model from OpenCV's calib3d/fisheye module.
106 *
107 * Parameters:
108 *
109 * \f[(k_1, k_2, k_3, k_4)\f]
110 *
111 * Many cameras use this model. Here's a non-exhaustive list of cameras Monado knows about that fall under this
112 * model:
113 * * Intel T265
114 * * Valve Index
115 */
117
118 /*!
119 * Windows Mixed Reality headsets' camera model.
120 *
121 * The model is listed as CALIBRATION_LensDistortionModelRational6KT in the WMR json files, which seems to be
122 * equivalent to Azure-Kinect-Sensor-SDK's K4A_CALIBRATION_LENS_DISTORTION_MODEL_RATIONAL_6KT.
123 *
124 * The only difference between this model and RT8 are codx, cody, and the way p1 and p2 are interpreted. In
125 * practice we reinterpret this as RT8 because those values are almost always approximately 0 for WMR headsets.
126 *
127 * Parameters:
128 *
129 * \f[(k_1, k_2, p_1, p_2, k_3, k_4, k_5, k_6, cod_x, cod_y, rpmax)\f]
130 */
132};
133
134
135/*!
136 * Stringifies a @ref t_camera_distortion_model
137 * @param model The distortion model to be stringified
138 * @return The distortion model as a string
139 */
140static inline const char *
142{
143 switch (model) {
144 case T_DISTORTION_OPENCV_RADTAN_5: return "T_DISTORTION_OPENCV_RADTAN_5"; break;
145 case T_DISTORTION_OPENCV_RADTAN_8: return "T_DISTORTION_OPENCV_RADTAN_8"; break;
146 case T_DISTORTION_OPENCV_RADTAN_14: return "T_DISTORTION_OPENCV_RADTAN_14"; break;
147 case T_DISTORTION_WMR: return "T_DISTORTION_WMR"; break;
148 case T_DISTORTION_FISHEYE_KB4: return "T_DISTORTION_FISHEYE_KB4"; break;
149 default: U_LOG_E("Invalid distortion_model! %d", model); return "INVALID";
150 }
151}
152
153/*!
154 * Returns the number of parameters needed for this @ref t_camera_distortion_model to be held by an OpenCV Mat and
155 * correctly interpreted by OpenCV's (un)projection functions.
156 *
157 * @param model The distortion model in question
158 * @return The number of distortion coefficients, or 0 if this model cannot be represented inside OpenCV.
159 */
160static inline size_t
162{
163 switch (model) {
164 case T_DISTORTION_OPENCV_RADTAN_5: return 5; break;
165 case T_DISTORTION_OPENCV_RADTAN_8: return 8; break;
166 case T_DISTORTION_OPENCV_RADTAN_14: return 14; break;
167 case T_DISTORTION_WMR: return 11; break;
168 case T_DISTORTION_FISHEYE_KB4: return 4; break;
169 default: U_LOG_E("Invalid distortion_model! %d", model); return 0;
170 }
171}
172
173static inline bool
174t_camera_distortion_model_is_fisheye(enum t_camera_distortion_model model)
175{
176 switch (model) {
177 case T_DISTORTION_FISHEYE_KB4: return true;
181 case T_DISTORTION_WMR: return false;
182 default: U_LOG_E("Invalid distortion_model! %d", model); return false;
183 }
184}
185
186/*!
187 * Parameters for @ref T_DISTORTION_OPENCV_RADTAN_5
188 * @ingroup aux_tracking
189 */
191{
192 double k1, k2, p1, p2, k3;
193};
194
195/*!
196 * Parameters for @ref T_DISTORTION_OPENCV_RADTAN_8
197 * @ingroup aux_tracking
198 */
200{
201 double k1, k2, p1, p2, k3, k4, k5, k6;
202};
203
204/*!
205 * Parameters for @ref T_DISTORTION_OPENCV_RADTAN_14
206 * @ingroup aux_tracking
207 */
209{
210 double k1, k2, p1, p2, k3, k4, k5, k6, s1, s2, s3, s4, tx, ty;
211};
212
213/*!
214 * Parameters for @ref T_DISTORTION_FISHEYE_KB4
215 * @ingroup aux_tracking
216 */
218{
219 double k1, k2, k3, k4;
220};
221
222/*!
223 * Parameters for @ref T_DISTORTION_WMR
224 * @ingroup aux_tracking
225 */
227{
228 double k1, k2, p1, p2, k3, k4, k5, k6, codx, cody, rpmax;
229};
230
231/*!
232 * @brief Essential calibration data for a single camera, or single lens/sensor
233 * of a stereo camera.
234 */
236{
237 //! Source image size
239
240 //! Camera intrinsics matrix
241 double intrinsics[3][3];
242
243 union {
249 double distortion_parameters_as_array[XRT_DISTORTION_MAX_DIM];
250 };
251
252
253 //! Distortion model that this camera uses.
255};
256
257/*!
258 * Stereo camera calibration data to be given to trackers.
259 */
261{
262 //! Ref counting
264
265 //! Calibration of individual views/sensor
267
268 //! Translation from first to second in the stereo pair.
270 //! Rotation matrix from first to second in the stereo pair.
271 double camera_rotation[3][3];
272
273 //! Essential matrix.
274 double camera_essential[3][3];
275 //! Fundamental matrix.
276 double camera_fundamental[3][3];
277};
278
279/*!
280 * Allocates a new stereo calibration data, unreferences the old data pointed to by @p out_c.
281 *
282 * @public @memberof t_stereo_camera_calibration
283 */
284void
287
288/*!
289 * Only to be called by @p t_stereo_camera_calibration_reference.
290 *
291 * @private @memberof t_stereo_camera_calibration
292 */
293void
294t_stereo_camera_calibration_destroy(struct t_stereo_camera_calibration *c);
295
296/*!
297 * Update the reference counts on a stereo calibration data(s).
298 *
299 * @param[in,out] dst Pointer to a object reference: if the object reference is
300 * non-null will decrement its counter. The reference that
301 * @p dst points to will be set to @p src.
302 * @param[in] src New object for @p dst to refer to (may be null).
303 * If non-null, will have its refcount increased.
304 *
305 * @relates t_stereo_camera_calibration
306 */
307static inline void
309{
310 struct t_stereo_camera_calibration *old_dst = *dst;
311
312 if (old_dst == src) {
313 return;
314 }
315
316 if (src) {
318 }
319
320 *dst = src;
321
322 if (old_dst) {
324 t_stereo_camera_calibration_destroy(old_dst);
325 }
326 }
327}
328
329/*!
330 * Small helper function that dumps one camera calibration data to logging.
331 *
332 * @relates t_camera_calibration
333 */
334void
336
337/*!
338 * Small helper function that dumps the stereo calibration data to logging.
339 *
340 * @relates t_stereo_camera_calibration
341 */
342void
344
345/*!
346 * Load stereo calibration data from a given file in v1 format (binary).
347 *
348 * @relates t_stereo_camera_calibration
349 */
350bool
351t_stereo_camera_calibration_load_v1(FILE *calib_file, struct t_stereo_camera_calibration **out_data);
352
353/*!
354 * Save the given stereo calibration data to the given file in v1 format (binary).
355 *
356 * @relates t_stereo_camera_calibration
357 */
358bool
360
361/*!
362 * Parse the json object in v2 format into stereo calibration data.
363 *
364 * @relates t_stereo_camera_calibration
365 */
366bool
368
369/*!
370 * Convert the given stereo calibration data into a json object in v2 format.
371 *
372 * @relates t_stereo_camera_calibration
373 */
374bool
376
377
378/*!
379 * Load stereo calibration data from a given file path.
380 *
381 * @relates t_stereo_camera_calibration
382 */
383bool
384t_stereo_camera_calibration_load(const char *calib_path, struct t_stereo_camera_calibration **out_data);
385
386/*!
387 * Save the given stereo calibration data to the given file path.
388 *
389 * @relates t_stereo_camera_calibration
390 */
391bool
392t_stereo_camera_calibration_save(const char *calib_path, struct t_stereo_camera_calibration *data);
393
394
395/*
396 *
397 * IMU calibration data.
398 *
399 */
400
401/*!
402 * @brief Parameters for accelerometer and gyroscope calibration.
403 * @see slam_tracker::imu_calibration for a more detailed description and references.
404 */
406{
407 //! Linear transformation for raw measurements alignment and scaling.
408 double transform[3][3];
409
410 //! Offset to apply to raw measurements.
411 double offset[3];
412
413 //! Modeled sensor bias. @see slam_tracker::imu_calibration.
414 double bias_std[3];
415
416 //! Modeled measurement noise. @see slam_tracker::imu_calibration.
417 double noise_std[3];
418};
419
420/*!
421 * @brief Combined IMU calibration data.
422 */
424{
425 //! Accelerometer calibration data.
427
428 //! Gyroscope calibration data.
430};
431/*!
432 * Prints a @ref t_inertial_calibration struct
433 *
434 * @relates t_camera_calibration
435 */
436void
438
439/*!
440 * Small helper function that dumps the imu calibration data to logging.
441 *
442 * @relates t_camera_calibration
443 */
444void
446
447
448/*
449 *
450 * Conversion functions.
451 *
452 */
453
455{
456 uint8_t v[256][256][256][3]; // nolint(readability-magic-numbers)
457};
458
459void
460t_convert_fill_table(struct t_convert_table *t);
461
462void
463t_convert_make_y8u8v8_to_r8g8b8(struct t_convert_table *t);
464
465void
466t_convert_make_y8u8v8_to_h8s8v8(struct t_convert_table *t);
467
468void
469t_convert_make_h8s8v8_to_r8g8b8(struct t_convert_table *t);
470
471void
472t_convert_in_place_y8u8v8_to_r8g8b8(uint32_t width, uint32_t height, size_t stride, void *data_ptr);
473
474void
475t_convert_in_place_y8u8v8_to_h8s8v8(uint32_t width, uint32_t height, size_t stride, void *data_ptr);
476
477void
478t_convert_in_place_h8s8v8_to_r8g8b8(uint32_t width, uint32_t height, size_t stride, void *data_ptr);
479
480
481/*
482 *
483 * Filter functions.
484 *
485 */
486
487#define T_HSV_SIZE 32
488#define T_HSV_STEP (256 / T_HSV_SIZE)
489
490#define T_HSV_DEFAULT_PARAMS() \
491 { \
492 { \
493 {165, 30, 160, 100}, \
494 {135, 30, 160, 100}, \
495 {95, 30, 160, 100}, \
496 }, \
497 {128, 80}, \
498 }
499
501{
502 uint8_t hue_min;
503 uint8_t hue_range;
504
505 uint8_t s_min;
506
507 uint8_t v_min;
508};
509
510/*!
511 * Parameters for constructing an HSV filter.
512 * @relates t_hsv_filter
513 */
515{
516 struct t_hsv_filter_color color[3];
517
518 struct
519 {
520 uint8_t s_max;
521 uint8_t v_min;
522 } white;
523};
524
526{
527 uint8_t v[256][256][256];
528};
529
531{
532 uint8_t v[T_HSV_SIZE][T_HSV_SIZE][T_HSV_SIZE];
533};
534
535void
536t_hsv_build_convert_table(struct t_hsv_filter_params *params, struct t_convert_table *t);
537
538void
539t_hsv_build_large_table(struct t_hsv_filter_params *params, struct t_hsv_filter_large_table *t);
540
541void
542t_hsv_build_optimized_table(struct t_hsv_filter_params *params, struct t_hsv_filter_optimized_table *t);
543
544static inline uint8_t
545t_hsv_filter_sample(struct t_hsv_filter_optimized_table *t, uint32_t y, uint32_t u, uint32_t v)
546{
547 return t->v[y / T_HSV_STEP][u / T_HSV_STEP][v / T_HSV_STEP];
548}
549
550/*!
551 * Construct an HSV filter sink.
552 * @public @memberof t_hsv_filter
553 *
554 * @see xrt_frame_context
555 */
556int
558 struct t_hsv_filter_params *params,
559 struct xrt_frame_sink *sinks[4],
560 struct xrt_frame_sink **out_sink);
561
562
563/*
564 *
565 * Tracker code.
566 *
567 */
568
569/*!
570 * @public @memberof xrt_tracked_psmv
571 */
572int
573t_psmv_start(struct xrt_tracked_psmv *xtmv);
574
575/*!
576 * @public @memberof xrt_tracked_psmv
577 */
578int
579t_psmv_create(struct xrt_frame_context *xfctx,
580 struct xrt_colour_rgb_f32 *rgb,
581 struct t_stereo_camera_calibration *data,
582 struct xrt_tracked_psmv **out_xtmv,
583 struct xrt_frame_sink **out_sink);
584
585/*!
586 * @public @memberof xrt_tracked_psvr
587 */
588int
589t_psvr_start(struct xrt_tracked_psvr *xtvr);
590
591/*!
592 * @public @memberof xrt_tracked_psvr
593 */
594int
595t_psvr_create(struct xrt_frame_context *xfctx,
596 struct t_stereo_camera_calibration *data,
597 struct xrt_tracked_psvr **out_xtvr,
598 struct xrt_frame_sink **out_sink);
599
600
601
602/*!
603 * SLAM prediction type. Naming scheme as follows:
604 * P: position, O: orientation, A: angular velocity, L: linear velocity
605 * S: From SLAM poses (slow, precise), I: From IMU data (fast, noisy)
606 *
607 * @see xrt_tracked_slam
608 */
610{
611 SLAM_PRED_NONE = 0, //!< No prediction, always return the last SLAM tracked pose
612 SLAM_PRED_SP_SO_SA_SL, //!< Predicts from last two SLAM poses only
613 SLAM_PRED_SP_SO_IA_SL, //!< Predicts from last SLAM pose with angular velocity computed from IMU
614 SLAM_PRED_SP_SO_IA_IL, //!< Predicts from last SLAM pose with angular and linear velocity computed from IMU
615 SLAM_PRED_IP_IO_IA_IL, //!< Predicts from a pose that is the last SLAM pose with the IMU samples that came after
616 //!< it integrated on top; velocities from latest IMU sample.
617 SLAM_PRED_COUNT,
618};
619
620/*!
621 * Extension to camera calibration for SLAM tracking
622 *
623 * @see xrt_tracked_slam
624 */
626{
627 struct t_camera_calibration base;
628 struct xrt_matrix_4x4 T_imu_cam; //!< Transform IMU to camera. Column major.
629 double frequency; //!< Camera FPS
630};
631
632/*!
633 * Extension to IMU calibration for SLAM tracking
634 *
635 * @see xrt_tracked_slam
636 */
638{
639 struct t_imu_calibration base;
640 double frequency;
641};
642
643/*!
644 * Calibration information necessary for SLAM tracking.
645 *
646 * @see xrt_tracked_slam
647 */
649{
650 struct t_slam_imu_calibration imu; //!< IMU calibration data
651 struct t_slam_camera_calibration cams[XRT_TRACKING_MAX_SLAM_CAMS]; //!< Calib data of `cam_count` cams
652 int cam_count; //!< Number of cameras
653};
654
655/*!
656 * SLAM tracker configuration.
657 *
658 * @see xrt_tracked_slam
659 */
661{
662 enum u_logging_level log_level; //!< SLAM tracking logging level
663 const char *vit_system_library_path; //!< Path to the VIT system library
664 const char *slam_config; //!< Config file path, format is specific to the SLAM implementation in use
665 int cam_count; //!< Number of cameras in use
666 bool slam_ui; //!< Whether to open the external UI of the external SLAM system
667 bool submit_from_start; //!< Whether to submit data to the SLAM tracker without user action
668 int openvr_groundtruth_device; //!< If >0, use lighthouse as groundtruth, see @ref openvr_device
669 enum t_slam_prediction_type prediction; //!< Which level of prediction to use
670 bool write_csvs; //!< Whether to enable CSV writers from the start for later analysis
671 const char *csv_path; //!< Path to write CSVs to
672 bool timing_stat; //!< Enable timing metric in external system
673 bool features_stat; //!< Enable feature metric in external system
674
675 //!< Instead of a slam_config file you can set custom calibration data
676 const struct t_slam_calibration *slam_calib;
677};
678
679/*!
680 * Fills in a @ref t_slam_tracker_config with default values.
681 *
682 * @see xrt_tracked_Slam
683 */
684void
686
687/*!
688 * @public @memberof xrt_tracked_slam
689 */
690int
691t_slam_create(struct xrt_frame_context *xfctx,
692 struct t_slam_tracker_config *config,
693 struct xrt_tracked_slam **out_xts,
694 struct xrt_slam_sinks **out_sink);
695
696/*!
697 * @public @memberof xrt_tracked_slam
698 */
699int
700t_slam_start(struct xrt_tracked_slam *xts);
701
702/*
703 *
704 * Camera calibration
705 *
706 */
707
708/*!
709 * Board pattern type.
710 */
712{
713 T_BOARD_CHECKERS,
714 //! Sector based checker board, using `cv::findChessboardCornersSB`.
716 T_BOARD_CIRCLES,
717 T_BOARD_ASYMMETRIC_CIRCLES,
718};
719
721{
722 //! Is calibration finished?
724 //! Was the target found this frame?
725 bool found;
726 //! Number of frames collected
728 //! Number of moving frames before another capture
730 //! Number of non-moving frames before capture.
732 //! Stereo calibration data that was produced.
734};
735
737{
738 //! Should we use fisheye version of the calibration functions.
740 //! Is the camera a stereo sbs camera, mostly for image loading.
742 //! What type of pattern are we using for calibration.
744
745 struct
746 {
747 int cols;
748 int rows;
749 float size_meters;
750
751 bool subpixel_enable;
752 int subpixel_size;
753 } checkers;
754
755 struct
756 {
757 int cols;
758 int rows;
759 float size_meters;
760
761 bool marker;
762 bool normalize_image;
763 } sb_checkers;
764
765 struct
766 {
767 int cols;
768 int rows;
769 float distance_meters;
770 } circles;
771
772 struct
773 {
774 int cols;
775 int rows;
776 float diagonal_distance_meters;
777 } asymmetric_circles;
778
779 struct
780 {
781 bool enabled;
782 int num_images;
783 } load;
784
785 int num_cooldown_frames;
786 int num_wait_for;
787 int num_collect_total;
788 int num_collect_restart;
789
790 /*!
791 * Should we mirror the RGB image?
792 *
793 * Before text is written out, has no effect on actual image capture.
794 */
796
797 bool save_images;
798};
799
800/*!
801 * Sets the calibration parameters to the their default values.
802 * @public @memberof t_calibration_params
803 */
804void
806
807void
808t_calibration_gui_params_load_or_default(struct t_calibration_params *p);
809
810void
811t_calibration_gui_params_to_json(cJSON **out_json, struct t_calibration_params *p);
812
813void
814t_calibration_gui_params_parse_from_json(const cJSON *params, struct t_calibration_params *p);
815
816/*!
817 * @brief Create the camera calibration frame sink.
818 *
819 * @param xfctx Context for frame transport.
820 * @param params Parameters to use during calibration. Values copied, pointer
821 * not retained.
822 * @param status Optional pointer to structure for status information. Pointer
823 * retained, and pointed-to struct modified.
824 * @param gui Frame sink
825 * @param out_sink Output: created frame sink.
826 *
827 * @see xrt_frame_context
828 */
829int
831 const struct t_calibration_params *params,
832 struct t_calibration_status *status,
833 struct xrt_frame_sink *gui,
834 struct xrt_frame_sink **out_sink);
835
836
837/*
838 *
839 * Sink creation functions.
840 *
841 */
842
843/*!
844 * @see xrt_frame_context
845 */
846int
848
849
850
851/*!
852 * @see xrt_frame_context
853 */
854int
856 struct xrt_frame_sink *passthrough,
857 struct xrt_frame_sink **out_sink);
858
859/*!
860 * @see xrt_frame_context
861 */
862int
864 struct xrt_frame_sink *passthrough,
865 struct xrt_frame_sink **out_sink);
866
867/*!
868 * @see xrt_frame_context
869 */
870int
872 struct xrt_frame_sink *passthrough,
873 struct xrt_frame_sink **out_sink);
874
875/*!
876 * @}
877 */
878
879
880#ifdef __cplusplus
881}
882#endif
u_logging_level
Logging level enum.
Definition: u_logging.h:44
#define U_LOG_E(...)
Log a message at U_LOGGING_ERROR level, conditional on the global log level.
Definition: u_logging.h:410
t_board_pattern
Board pattern type.
Definition: t_tracking.h:712
t_slam_prediction_type
SLAM prediction type.
Definition: t_tracking.h:610
void t_slam_fill_default_config(struct t_slam_tracker_config *config)
Fills in a t_slam_tracker_config with default values.
Definition: t_tracker_slam.cpp:1495
int t_convert_yuv_or_yuyv_create(struct xrt_frame_sink *next, struct xrt_frame_sink **out_sink)
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).
Definition: t_file.cpp:507
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).
Definition: t_file.cpp:227
int t_debug_hsv_filter_create(struct xrt_frame_context *xfctx, struct xrt_frame_sink *passthrough, struct xrt_frame_sink **out_sink)
Definition: t_debug_hsv_filter.cpp:107
static const char * t_stringify_camera_distortion_model(const enum t_camera_distortion_model model)
Stringifies a t_camera_distortion_model.
Definition: t_tracking.h:141
#define XRT_DISTORTION_MAX_DIM
Maximum size of rectilinear distortion coefficient array.
Definition: t_tracking.h:54
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.
Definition: t_file.cpp:606
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.
Definition: t_hsv_filter.c:328
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 Ma...
Definition: t_tracking.h:161
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.
Definition: t_file.cpp:811
int t_debug_hsv_viewer_create(struct xrt_frame_context *xfctx, struct xrt_frame_sink *passthrough, struct xrt_frame_sink **out_sink)
Definition: t_debug_hsv_viewer.cpp:168
void t_imu_calibration_dump(struct t_imu_calibration *c)
Small helper function that dumps the imu calibration data to logging.
Definition: t_data_utils.c:369
void t_inertial_calibration_dump(struct t_inertial_calibration *c)
Prints a t_inertial_calibration struct.
Definition: t_data_utils.c:360
void t_stereo_camera_calibration_dump(struct t_stereo_camera_calibration *c)
Small helper function that dumps the stereo calibration data to logging.
Definition: t_data_utils.c:138
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.
Definition: t_file.cpp:818
int t_debug_hsv_picker_create(struct xrt_frame_context *xfctx, struct xrt_frame_sink *passthrough, struct xrt_frame_sink **out_sink)
Definition: t_debug_hsv_picker.cpp:219
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).
Definition: t_tracking.h:308
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.
void t_calibration_gui_params_default(struct t_calibration_params *p)
Sets the calibration parameters to the their default values.
Definition: t_data_utils.c:284
t_camera_distortion_model
The distortion model this camera calibration falls under.
Definition: t_tracking.h:63
void t_camera_calibration_dump(struct t_camera_calibration *c)
Small helper function that dumps one camera calibration data to logging.
Definition: t_data_utils.c:128
void 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.
Definition: t_data_utils.c:112
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.
Definition: t_file.cpp:435
@ T_BOARD_SB_CHECKERS
Sector based checker board, using cv::findChessboardCornersSB.
Definition: t_tracking.h:715
@ 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...
Definition: t_tracking.h:615
@ SLAM_PRED_SP_SO_SA_SL
Predicts from last two SLAM poses only.
Definition: t_tracking.h:612
@ SLAM_PRED_SP_SO_IA_SL
Predicts from last SLAM pose with angular velocity computed from IMU.
Definition: t_tracking.h:613
@ SLAM_PRED_NONE
No prediction, always return the last SLAM tracked pose.
Definition: t_tracking.h:611
@ SLAM_PRED_SP_SO_IA_IL
Predicts from last SLAM pose with angular and linear velocity computed from IMU.
Definition: t_tracking.h:614
@ T_DISTORTION_OPENCV_RADTAN_8
OpenCV's radial-tangential distortion model.
Definition: t_tracking.h:82
@ T_DISTORTION_OPENCV_RADTAN_5
OpenCV's radial-tangential distortion model.
Definition: t_tracking.h:73
@ T_DISTORTION_FISHEYE_KB4
Juho Kannalla and Sami Sebastian Brandt's fisheye distortion model.
Definition: t_tracking.h:116
@ T_DISTORTION_WMR
Windows Mixed Reality headsets' camera model.
Definition: t_tracking.h:131
@ T_DISTORTION_OPENCV_RADTAN_14
OpenCV's radial-tangential distortion model.
Definition: t_tracking.h:101
static XRT_CHECK_RESULT bool xrt_reference_dec_and_is_zero(struct xrt_reference *xref)
Decrement the reference and return true if the value is now zero.
Definition: xrt_defines.h:2223
static void xrt_reference_inc(struct xrt_reference *xref)
Increment the reference, probably want xrt_reference_inc_and_was_zero.
Definition: xrt_defines.h:2179
Definition: t_tracking.h:737
bool stereo_sbs
Is the camera a stereo sbs camera, mostly for image loading.
Definition: t_tracking.h:741
bool use_fisheye
Should we use fisheye version of the calibration functions.
Definition: t_tracking.h:739
enum t_board_pattern pattern
What type of pattern are we using for calibration.
Definition: t_tracking.h:743
bool mirror_rgb_image
Should we mirror the RGB image?
Definition: t_tracking.h:795
Definition: t_tracking.h:721
bool finished
Is calibration finished?
Definition: t_tracking.h:723
int num_collected
Number of frames collected.
Definition: t_tracking.h:727
struct t_stereo_camera_calibration * stereo_data
Stereo calibration data that was produced.
Definition: t_tracking.h:733
int waits_remaining
Number of non-moving frames before capture.
Definition: t_tracking.h:731
int cooldown
Number of moving frames before another capture.
Definition: t_tracking.h:729
bool found
Was the target found this frame?
Definition: t_tracking.h:725
Parameters for T_DISTORTION_FISHEYE_KB4.
Definition: t_tracking.h:218
Parameters for T_DISTORTION_OPENCV_RADTAN_14.
Definition: t_tracking.h:209
Parameters for T_DISTORTION_OPENCV_RADTAN_5.
Definition: t_tracking.h:191
Parameters for T_DISTORTION_OPENCV_RADTAN_8.
Definition: t_tracking.h:200
Parameters for T_DISTORTION_WMR.
Definition: t_tracking.h:227
Essential calibration data for a single camera, or single lens/sensor of a stereo camera.
Definition: t_tracking.h:236
double intrinsics[3][3]
Camera intrinsics matrix.
Definition: t_tracking.h:241
struct xrt_size image_size_pixels
Source image size.
Definition: t_tracking.h:238
enum t_camera_distortion_model distortion_model
Distortion model that this camera uses.
Definition: t_tracking.h:254
Definition: t_tracking.h:455
Definition: t_tracking.h:501
Definition: t_tracking.h:526
Definition: t_tracking.h:531
Parameters for constructing an HSV filter.
Definition: t_tracking.h:515
Combined IMU calibration data.
Definition: t_tracking.h:424
struct t_inertial_calibration accel
Accelerometer calibration data.
Definition: t_tracking.h:426
struct t_inertial_calibration gyro
Gyroscope calibration data.
Definition: t_tracking.h:429
Parameters for accelerometer and gyroscope calibration.
Definition: t_tracking.h:406
double bias_std[3]
Modeled sensor bias.
Definition: t_tracking.h:414
double noise_std[3]
Modeled measurement noise.
Definition: t_tracking.h:417
double offset[3]
Offset to apply to raw measurements.
Definition: t_tracking.h:411
double transform[3][3]
Linear transformation for raw measurements alignment and scaling.
Definition: t_tracking.h:408
Calibration information necessary for SLAM tracking.
Definition: t_tracking.h:649
struct t_slam_camera_calibration cams[XRT_TRACKING_MAX_SLAM_CAMS]
Calib data of cam_count cams.
Definition: t_tracking.h:651
int cam_count
Number of cameras.
Definition: t_tracking.h:652
struct t_slam_imu_calibration imu
IMU calibration data.
Definition: t_tracking.h:650
Extension to camera calibration for SLAM tracking.
Definition: t_tracking.h:626
struct xrt_matrix_4x4 T_imu_cam
Transform IMU to camera.
Definition: t_tracking.h:628
double frequency
Camera FPS.
Definition: t_tracking.h:629
Extension to IMU calibration for SLAM tracking.
Definition: t_tracking.h:638
SLAM tracker configuration.
Definition: t_tracking.h:661
bool features_stat
Enable feature metric in external system.
Definition: t_tracking.h:673
bool submit_from_start
Whether to submit data to the SLAM tracker without user action.
Definition: t_tracking.h:667
enum t_slam_prediction_type prediction
Which level of prediction to use.
Definition: t_tracking.h:669
const char * vit_system_library_path
Path to the VIT system library.
Definition: t_tracking.h:663
const char * csv_path
Path to write CSVs to.
Definition: t_tracking.h:671
int cam_count
Number of cameras in use.
Definition: t_tracking.h:665
int openvr_groundtruth_device
If >0, use lighthouse as groundtruth, see openvr_device.
Definition: t_tracking.h:668
enum u_logging_level log_level
SLAM tracking logging level.
Definition: t_tracking.h:662
const char * slam_config
Config file path, format is specific to the SLAM implementation in use.
Definition: t_tracking.h:664
bool write_csvs
Whether to enable CSV writers from the start for later analysis.
Definition: t_tracking.h:670
bool timing_stat
Enable timing metric in external system.
Definition: t_tracking.h:672
bool slam_ui
Whether to open the external UI of the external SLAM system.
Definition: t_tracking.h:666
Stereo camera calibration data to be given to trackers.
Definition: t_tracking.h:261
double camera_essential[3][3]
Essential matrix.
Definition: t_tracking.h:274
double camera_translation[3]
Translation from first to second in the stereo pair.
Definition: t_tracking.h:269
struct t_camera_calibration view[2]
Calibration of individual views/sensor.
Definition: t_tracking.h:266
double camera_rotation[3][3]
Rotation matrix from first to second in the stereo pair.
Definition: t_tracking.h:271
double camera_fundamental[3][3]
Fundamental matrix.
Definition: t_tracking.h:276
struct xrt_reference reference
Ref counting.
Definition: t_tracking.h:263
A 3 element colour with floating point channels.
Definition: xrt_defines.h:383
Object used to track all sinks and frame producers in a graph.
Definition: xrt_frame.h:108
A object that is sent frames.
Definition: xrt_frame.h:58
A tightly packed 4x4 matrix of floats.
Definition: xrt_defines.h:558
A base class for reference counted objects.
Definition: xrt_defines.h:98
Image size.
Definition: xrt_defines.h:408
Container of pointers to sinks that could be used for a SLAM system.
Definition: xrt_tracking.h:202
A single tracked PS Move controller, camera and ball are not synced.
Definition: xrt_tracking.h:218
A tracked PSVR headset.
Definition: xrt_tracking.h:260
An adapter that wraps an external SLAM tracker to provide SLAM tracking.
Definition: xrt_tracking.h:294
Basic logging functionality.
Very small misc utils.
Common defines and enums for XRT.
Data frame header.
Header defining the tracking system integration in Monado.