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 Moses Turner <moses@collabora.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
25 extern "C" {
26 #endif
27 
28 /*!
29  * @addtogroup aux_tracking
30  * @{
31  */
32 
33 
34 /*
35  *
36  * Pre-declare
37  *
38  */
39 
40 typedef struct cJSON cJSON;
41 struct xrt_slam_sinks;
42 struct xrt_tracked_psmv;
43 struct xrt_tracked_psvr;
44 struct 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  */
140 static 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  */
160 static 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 
173 /*!
174  * Parameters for @ref T_DISTORTION_OPENCV_RADTAN_5
175  * @ingroup aux_tracking
176  */
178 {
179  double k1, k2, p1, p2, k3;
180 };
181 
182 /*!
183  * Parameters for @ref T_DISTORTION_OPENCV_RADTAN_8
184  * @ingroup aux_tracking
185  */
187 {
188  double k1, k2, p1, p2, k3, k4, k5, k6;
189 };
190 
191 /*!
192  * Parameters for @ref T_DISTORTION_OPENCV_RADTAN_14
193  * @ingroup aux_tracking
194  */
196 {
197  double k1, k2, p1, p2, k3, k4, k5, k6, s1, s2, s3, s4, tx, ty;
198 };
199 
200 /*!
201  * Parameters for @ref T_DISTORTION_FISHEYE_KB4
202  * @ingroup aux_tracking
203  */
205 {
206  double k1, k2, k3, k4;
207 };
208 
209 /*!
210  * Parameters for @ref T_DISTORTION_WMR
211  * @ingroup aux_tracking
212  */
214 {
215  double k1, k2, p1, p2, k3, k4, k5, k6, codx, cody, rpmax;
216 };
217 
218 /*!
219  * @brief Essential calibration data for a single camera, or single lens/sensor
220  * of a stereo camera.
221  */
223 {
224  //! Source image size
226 
227  //! Camera intrinsics matrix
228  double intrinsics[3][3];
229 
230  union {
236  double distortion_parameters_as_array[XRT_DISTORTION_MAX_DIM];
237  };
238 
239 
240  //! Distortion model that this camera uses.
242 };
243 
244 /*!
245  * Stereo camera calibration data to be given to trackers.
246  */
248 {
249  //! Ref counting
250  struct xrt_reference reference;
251 
252  //! Calibration of individual views/sensor
253  struct t_camera_calibration view[2];
254 
255  //! Translation from first to second in the stereo pair.
257  //! Rotation matrix from first to second in the stereo pair.
258  double camera_rotation[3][3];
259 
260  //! Essential matrix.
261  double camera_essential[3][3];
262  //! Fundamental matrix.
263  double camera_fundamental[3][3];
264 };
265 
266 /*!
267  * Allocates a new stereo calibration data, unreferences the old data pointed to by @p out_c.
268  *
269  * @public @memberof t_stereo_camera_calibration
270  */
271 void
274 
275 /*!
276  * Only to be called by @p t_stereo_camera_calibration_reference.
277  *
278  * @private @memberof t_stereo_camera_calibration
279  */
280 void
281 t_stereo_camera_calibration_destroy(struct t_stereo_camera_calibration *c);
282 
283 /*!
284  * Update the reference counts on a stereo calibration data(s).
285  *
286  * @param[in,out] dst Pointer to a object reference: if the object reference is
287  * non-null will decrement its counter. The reference that
288  * @p dst points to will be set to @p src.
289  * @param[in] src New object for @p dst to refer to (may be null).
290  * If non-null, will have its refcount increased.
291  *
292  * @relates t_stereo_camera_calibration
293  */
294 static inline void
296 {
297  struct t_stereo_camera_calibration *old_dst = *dst;
298 
299  if (old_dst == src) {
300  return;
301  }
302 
303  if (src) {
305  }
306 
307  *dst = src;
308 
309  if (old_dst) {
310  if (xrt_reference_dec_and_is_zero(&old_dst->reference)) {
311  t_stereo_camera_calibration_destroy(old_dst);
312  }
313  }
314 }
315 
316 /*!
317  * Small helper function that dumps one camera calibration data to logging.
318  *
319  * @relates t_camera_calibration
320  */
321 void
323 
324 /*!
325  * Small helper function that dumps the stereo calibration data to logging.
326  *
327  * @relates t_stereo_camera_calibration
328  */
329 void
331 
332 /*!
333  * Load stereo calibration data from a given file in v1 format (binary).
334  *
335  * @relates t_stereo_camera_calibration
336  */
337 bool
338 t_stereo_camera_calibration_load_v1(FILE *calib_file, struct t_stereo_camera_calibration **out_data);
339 
340 /*!
341  * Save the given stereo calibration data to the given file in v1 format (binary).
342  *
343  * @relates t_stereo_camera_calibration
344  */
345 bool
347 
348 /*!
349  * Parse the json object in v2 format into stereo calibration data.
350  *
351  * @relates t_stereo_camera_calibration
352  */
353 bool
355 
356 /*!
357  * Convert the given stereo calibration data into a json object in v2 format.
358  *
359  * @relates t_stereo_camera_calibration
360  */
361 bool
363 
364 
365 /*!
366  * Load stereo calibration data from a given file path.
367  *
368  * @relates t_stereo_camera_calibration
369  */
370 bool
371 t_stereo_camera_calibration_load(const char *calib_path, struct t_stereo_camera_calibration **out_data);
372 
373 /*!
374  * Save the given stereo calibration data to the given file path.
375  *
376  * @relates t_stereo_camera_calibration
377  */
378 bool
379 t_stereo_camera_calibration_save(const char *calib_path, struct t_stereo_camera_calibration *data);
380 
381 
382 /*
383  *
384  * IMU calibration data.
385  *
386  */
387 
388 /*!
389  * @brief Parameters for accelerometer and gyroscope calibration.
390  * @see slam_tracker::imu_calibration for a more detailed description and references.
391  */
393 {
394  //! Linear transformation for raw measurements alignment and scaling.
395  double transform[3][3];
396 
397  //! Offset to apply to raw measurements.
398  double offset[3];
399 
400  //! Modeled sensor bias. @see slam_tracker::imu_calibration.
401  double bias_std[3];
402 
403  //! Modeled measurement noise. @see slam_tracker::imu_calibration.
404  double noise_std[3];
405 };
406 
407 /*!
408  * @brief Combined IMU calibration data.
409  */
411 {
412  //! Accelerometer calibration data.
414 
415  //! Gyroscope calibration data.
417 };
418 /*!
419  * Prints a @ref t_inertial_calibration struct
420  *
421  * @relates t_camera_calibration
422  */
423 void
425 
426 /*!
427  * Small helper function that dumps the imu calibration data to logging.
428  *
429  * @relates t_camera_calibration
430  */
431 void
433 
434 
435 /*
436  *
437  * Conversion functions.
438  *
439  */
440 
442 {
443  uint8_t v[256][256][256][3]; // nolint(readability-magic-numbers)
444 };
445 
446 void
447 t_convert_fill_table(struct t_convert_table *t);
448 
449 void
450 t_convert_make_y8u8v8_to_r8g8b8(struct t_convert_table *t);
451 
452 void
453 t_convert_make_y8u8v8_to_h8s8v8(struct t_convert_table *t);
454 
455 void
456 t_convert_make_h8s8v8_to_r8g8b8(struct t_convert_table *t);
457 
458 void
459 t_convert_in_place_y8u8v8_to_r8g8b8(uint32_t width, uint32_t height, size_t stride, void *data_ptr);
460 
461 void
462 t_convert_in_place_y8u8v8_to_h8s8v8(uint32_t width, uint32_t height, size_t stride, void *data_ptr);
463 
464 void
465 t_convert_in_place_h8s8v8_to_r8g8b8(uint32_t width, uint32_t height, size_t stride, void *data_ptr);
466 
467 
468 /*
469  *
470  * Filter functions.
471  *
472  */
473 
474 #define T_HSV_SIZE 32
475 #define T_HSV_STEP (256 / T_HSV_SIZE)
476 
477 #define T_HSV_DEFAULT_PARAMS() \
478  { \
479  { \
480  {165, 30, 160, 100}, \
481  {135, 30, 160, 100}, \
482  {95, 30, 160, 100}, \
483  }, \
484  {128, 80}, \
485  }
486 
488 {
489  uint8_t hue_min;
490  uint8_t hue_range;
491 
492  uint8_t s_min;
493 
494  uint8_t v_min;
495 };
496 
497 /*!
498  * Parameters for constructing an HSV filter.
499  * @relates t_hsv_filter
500  */
502 {
503  struct t_hsv_filter_color color[3];
504 
505  struct
506  {
507  uint8_t s_max;
508  uint8_t v_min;
509  } white;
510 };
511 
513 {
514  uint8_t v[256][256][256];
515 };
516 
518 {
519  uint8_t v[T_HSV_SIZE][T_HSV_SIZE][T_HSV_SIZE];
520 };
521 
522 void
523 t_hsv_build_convert_table(struct t_hsv_filter_params *params, struct t_convert_table *t);
524 
525 void
526 t_hsv_build_large_table(struct t_hsv_filter_params *params, struct t_hsv_filter_large_table *t);
527 
528 void
529 t_hsv_build_optimized_table(struct t_hsv_filter_params *params, struct t_hsv_filter_optimized_table *t);
530 
531 static inline uint8_t
532 t_hsv_filter_sample(struct t_hsv_filter_optimized_table *t, uint32_t y, uint32_t u, uint32_t v)
533 {
534  return t->v[y / T_HSV_STEP][u / T_HSV_STEP][v / T_HSV_STEP];
535 }
536 
537 /*!
538  * Construct an HSV filter sink.
539  * @public @memberof t_hsv_filter
540  *
541  * @see xrt_frame_context
542  */
543 int
545  struct t_hsv_filter_params *params,
546  struct xrt_frame_sink *sinks[4],
547  struct xrt_frame_sink **out_sink);
548 
549 
550 /*
551  *
552  * Tracker code.
553  *
554  */
555 
556 /*!
557  * @public @memberof xrt_tracked_psmv
558  */
559 int
560 t_psmv_start(struct xrt_tracked_psmv *xtmv);
561 
562 /*!
563  * @public @memberof xrt_tracked_psmv
564  */
565 int
566 t_psmv_create(struct xrt_frame_context *xfctx,
567  struct xrt_colour_rgb_f32 *rgb,
568  struct t_stereo_camera_calibration *data,
569  struct xrt_tracked_psmv **out_xtmv,
570  struct xrt_frame_sink **out_sink);
571 
572 /*!
573  * @public @memberof xrt_tracked_psvr
574  */
575 int
576 t_psvr_start(struct xrt_tracked_psvr *xtvr);
577 
578 /*!
579  * @public @memberof xrt_tracked_psvr
580  */
581 int
582 t_psvr_create(struct xrt_frame_context *xfctx,
583  struct t_stereo_camera_calibration *data,
584  struct xrt_tracked_psvr **out_xtvr,
585  struct xrt_frame_sink **out_sink);
586 
587 
588 
589 /*!
590  * SLAM prediction type. Naming scheme as follows:
591  * P: position, O: orientation, A: angular velocity, L: linear velocity
592  * S: From SLAM poses (slow, precise), I: From IMU data (fast, noisy)
593  *
594  * @see xrt_tracked_slam
595  */
597 {
598  SLAM_PRED_NONE = 0, //!< No prediction, always return the last SLAM tracked pose
599  SLAM_PRED_SP_SO_SA_SL, //!< Predicts from last two SLAM poses only
600  SLAM_PRED_SP_SO_IA_SL, //!< Predicts from last SLAM pose with angular velocity computed from IMU
601  SLAM_PRED_SP_SO_IA_IL, //!< Predicts from last SLAM pose with angular and linear velocity computed from IMU
602  SLAM_PRED_IP_IO_IA_IL, //!< Predicts from a pose that is the last SLAM pose with the IMU samples that came after
603  //!< it integrated on top; velocities from latest IMU sample.
604  SLAM_PRED_COUNT,
605 };
606 
607 /*!
608  * Extension to camera calibration for SLAM tracking
609  *
610  * @see xrt_tracked_slam
611  */
613 {
614  struct t_camera_calibration base;
615  struct xrt_matrix_4x4 T_imu_cam; //!< Transform IMU to camera. Column major.
616  double frequency; //!< Camera FPS
617 };
618 
619 /*!
620  * Extension to IMU calibration for SLAM tracking
621  *
622  * @see xrt_tracked_slam
623  */
625 {
626  struct t_imu_calibration base;
627  double frequency;
628 };
629 
630 /*!
631  * Calibration information necessary for SLAM tracking.
632  *
633  * @see xrt_tracked_slam
634  */
636 {
637  struct t_slam_imu_calibration imu; //!< IMU calibration data
638  struct t_slam_camera_calibration cams[XRT_TRACKING_MAX_SLAM_CAMS]; //!< Calib data of `cam_count` cams
639  int cam_count; //!< Number of cameras
640 };
641 
642 /*!
643  * SLAM tracker configuration.
644  *
645  * @see xrt_tracked_slam
646  */
648 {
649  enum u_logging_level log_level; //!< SLAM tracking logging level
650  const char *vit_system_library_path; //!< Path to the VIT system library
651  const char *slam_config; //!< Config file path, format is specific to the SLAM implementation in use
652  int cam_count; //!< Number of cameras in use
653  bool slam_ui; //!< Whether to open the external UI of the external SLAM system
654  bool submit_from_start; //!< Whether to submit data to the SLAM tracker without user action
655  int openvr_groundtruth_device; //!< If >0, use lighthouse as groundtruth, see @ref openvr_device
656  enum t_slam_prediction_type prediction; //!< Which level of prediction to use
657  bool write_csvs; //!< Whether to enable CSV writers from the start for later analysis
658  const char *csv_path; //!< Path to write CSVs to
659  bool timing_stat; //!< Enable timing metric in external system
660  bool features_stat; //!< Enable feature metric in external system
661 
662  //!< Instead of a slam_config file you can set custom calibration data
663  const struct t_slam_calibration *slam_calib;
664 };
665 
666 /*!
667  * Fills in a @ref t_slam_tracker_config with default values.
668  *
669  * @see xrt_tracked_Slam
670  */
671 void
673 
674 /*!
675  * @public @memberof xrt_tracked_slam
676  */
677 int
678 t_slam_create(struct xrt_frame_context *xfctx,
679  struct t_slam_tracker_config *config,
680  struct xrt_tracked_slam **out_xts,
681  struct xrt_slam_sinks **out_sink);
682 
683 /*!
684  * @public @memberof xrt_tracked_slam
685  */
686 int
687 t_slam_start(struct xrt_tracked_slam *xts);
688 
689 /*
690  *
691  * Camera calibration
692  *
693  */
694 
695 /*!
696  * Board pattern type.
697  */
699 {
700  T_BOARD_CHECKERS,
701  //! Sector based checker board, using `cv::findChessboardCornersSB`.
703  T_BOARD_CIRCLES,
704  T_BOARD_ASYMMETRIC_CIRCLES,
705 };
706 
708 {
709  //! Is calibration finished?
710  bool finished;
711  //! Was the target found this frame?
712  bool found;
713  //! Number of frames collected
715  //! Number of moving frames before another capture
716  int cooldown;
717  //! Number of non-moving frames before capture.
719  //! Stereo calibration data that was produced.
721 };
722 
724 {
725  //! Should we use fisheye version of the calibration functions.
727  //! Is the camera a stereo sbs camera, mostly for image loading.
729  //! What type of pattern are we using for calibration.
731 
732  struct
733  {
734  int cols;
735  int rows;
736  float size_meters;
737 
738  bool subpixel_enable;
739  int subpixel_size;
740  } checkers;
741 
742  struct
743  {
744  int cols;
745  int rows;
746  float size_meters;
747 
748  bool marker;
749  bool normalize_image;
750  } sb_checkers;
751 
752  struct
753  {
754  int cols;
755  int rows;
756  float distance_meters;
757  } circles;
758 
759  struct
760  {
761  int cols;
762  int rows;
763  float diagonal_distance_meters;
764  } asymmetric_circles;
765 
766  struct
767  {
768  bool enabled;
769  int num_images;
770  } load;
771 
772  int num_cooldown_frames;
773  int num_wait_for;
774  int num_collect_total;
775  int num_collect_restart;
776 
777  /*!
778  * Should we mirror the RGB image?
779  *
780  * Before text is written out, has no effect on actual image capture.
781  */
783 
784  bool save_images;
785 };
786 
787 /*!
788  * Sets the calibration parameters to the their default values.
789  * @public @memberof t_calibration_params
790  */
791 void
793 
794 void
795 t_calibration_gui_params_load_or_default(struct t_calibration_params *p);
796 
797 void
798 t_calibration_gui_params_to_json(cJSON **out_json, struct t_calibration_params *p);
799 
800 void
801 t_calibration_gui_params_parse_from_json(const cJSON *params, struct t_calibration_params *p);
802 
803 /*!
804  * @brief Create the camera calibration frame sink.
805  *
806  * @param xfctx Context for frame transport.
807  * @param params Parameters to use during calibration. Values copied, pointer
808  * not retained.
809  * @param status Optional pointer to structure for status information. Pointer
810  * retained, and pointed-to struct modified.
811  * @param gui Frame sink
812  * @param out_sink Output: created frame sink.
813  *
814  * @see xrt_frame_context
815  */
816 int
818  const struct t_calibration_params *params,
819  struct t_calibration_status *status,
820  struct xrt_frame_sink *gui,
821  struct xrt_frame_sink **out_sink);
822 
823 
824 /*
825  *
826  * Sink creation functions.
827  *
828  */
829 
830 /*!
831  * @see xrt_frame_context
832  */
833 int
835 
836 
837 
838 /*!
839  * @see xrt_frame_context
840  */
841 int
843  struct xrt_frame_sink *passthrough,
844  struct xrt_frame_sink **out_sink);
845 
846 /*!
847  * @see xrt_frame_context
848  */
849 int
851  struct xrt_frame_sink *passthrough,
852  struct xrt_frame_sink **out_sink);
853 
854 /*!
855  * @see xrt_frame_context
856  */
857 int
859  struct xrt_frame_sink *passthrough,
860  struct xrt_frame_sink **out_sink);
861 
862 /*!
863  * @}
864  */
865 
866 
867 #ifdef __cplusplus
868 }
869 #endif
u_logging_level
Logging level enum.
Definition: u_logging.h:40
#define U_LOG_E(...)
Log a message at U_LOGGING_ERROR level, conditional on the global log level.
Definition: u_logging.h:301
t_board_pattern
Board pattern type.
Definition: t_tracking.h:699
t_slam_prediction_type
SLAM prediction type.
Definition: t_tracking.h:597
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:1502
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:508
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
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:228
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
#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:607
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:812
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:819
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:295
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:436
@ T_BOARD_SB_CHECKERS
Sector based checker board, using cv::findChessboardCornersSB.
Definition: t_tracking.h:702
@ 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:602
@ SLAM_PRED_SP_SO_SA_SL
Predicts from last two SLAM poses only.
Definition: t_tracking.h:599
@ SLAM_PRED_SP_SO_IA_SL
Predicts from last SLAM pose with angular velocity computed from IMU.
Definition: t_tracking.h:600
@ SLAM_PRED_NONE
No prediction, always return the last SLAM tracked pose.
Definition: t_tracking.h:598
@ SLAM_PRED_SP_SO_IA_IL
Predicts from last SLAM pose with angular and linear velocity computed from IMU.
Definition: t_tracking.h:601
@ 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:1598
static void xrt_reference_inc(struct xrt_reference *xref)
Increment the reference, probably want xrt_reference_inc_and_was_zero.
Definition: xrt_defines.h:1561
Definition: t_tracking.h:724
bool stereo_sbs
Is the camera a stereo sbs camera, mostly for image loading.
Definition: t_tracking.h:728
bool use_fisheye
Should we use fisheye version of the calibration functions.
Definition: t_tracking.h:726
enum t_board_pattern pattern
What type of pattern are we using for calibration.
Definition: t_tracking.h:730
bool mirror_rgb_image
Should we mirror the RGB image?
Definition: t_tracking.h:782
Definition: t_tracking.h:708
bool finished
Is calibration finished?
Definition: t_tracking.h:710
int num_collected
Number of frames collected.
Definition: t_tracking.h:714
struct t_stereo_camera_calibration * stereo_data
Stereo calibration data that was produced.
Definition: t_tracking.h:720
int waits_remaining
Number of non-moving frames before capture.
Definition: t_tracking.h:718
int cooldown
Number of moving frames before another capture.
Definition: t_tracking.h:716
bool found
Was the target found this frame?
Definition: t_tracking.h:712
Parameters for T_DISTORTION_FISHEYE_KB4.
Definition: t_tracking.h:205
Parameters for T_DISTORTION_OPENCV_RADTAN_14.
Definition: t_tracking.h:196
Parameters for T_DISTORTION_OPENCV_RADTAN_5.
Definition: t_tracking.h:178
Parameters for T_DISTORTION_OPENCV_RADTAN_8.
Definition: t_tracking.h:187
Parameters for T_DISTORTION_WMR.
Definition: t_tracking.h:214
Essential calibration data for a single camera, or single lens/sensor of a stereo camera.
Definition: t_tracking.h:223
double intrinsics[3][3]
Camera intrinsics matrix.
Definition: t_tracking.h:228
struct xrt_size image_size_pixels
Source image size.
Definition: t_tracking.h:225
enum t_camera_distortion_model distortion_model
Distortion model that this camera uses.
Definition: t_tracking.h:241
Definition: t_tracking.h:442
Definition: t_tracking.h:488
Definition: t_tracking.h:513
Definition: t_tracking.h:518
Parameters for constructing an HSV filter.
Definition: t_tracking.h:502
Combined IMU calibration data.
Definition: t_tracking.h:411
struct t_inertial_calibration accel
Accelerometer calibration data.
Definition: t_tracking.h:413
struct t_inertial_calibration gyro
Gyroscope calibration data.
Definition: t_tracking.h:416
Parameters for accelerometer and gyroscope calibration.
Definition: t_tracking.h:393
double bias_std[3]
Modeled sensor bias.
Definition: t_tracking.h:401
double noise_std[3]
Modeled measurement noise.
Definition: t_tracking.h:404
double offset[3]
Offset to apply to raw measurements.
Definition: t_tracking.h:398
double transform[3][3]
Linear transformation for raw measurements alignment and scaling.
Definition: t_tracking.h:395
Calibration information necessary for SLAM tracking.
Definition: t_tracking.h:636
struct t_slam_camera_calibration cams[XRT_TRACKING_MAX_SLAM_CAMS]
Calib data of cam_count cams.
Definition: t_tracking.h:638
int cam_count
Number of cameras.
Definition: t_tracking.h:639
struct t_slam_imu_calibration imu
IMU calibration data.
Definition: t_tracking.h:637
Extension to camera calibration for SLAM tracking.
Definition: t_tracking.h:613
struct xrt_matrix_4x4 T_imu_cam
Transform IMU to camera.
Definition: t_tracking.h:615
double frequency
Camera FPS.
Definition: t_tracking.h:616
Extension to IMU calibration for SLAM tracking.
Definition: t_tracking.h:625
SLAM tracker configuration.
Definition: t_tracking.h:648
bool features_stat
Enable feature metric in external system.
Definition: t_tracking.h:660
bool submit_from_start
Whether to submit data to the SLAM tracker without user action.
Definition: t_tracking.h:654
enum t_slam_prediction_type prediction
Which level of prediction to use.
Definition: t_tracking.h:656
const char * vit_system_library_path
Path to the VIT system library.
Definition: t_tracking.h:650
const char * csv_path
Path to write CSVs to.
Definition: t_tracking.h:658
int cam_count
Number of cameras in use.
Definition: t_tracking.h:652
int openvr_groundtruth_device
If >0, use lighthouse as groundtruth, see openvr_device.
Definition: t_tracking.h:655
enum u_logging_level log_level
SLAM tracking logging level.
Definition: t_tracking.h:649
const char * slam_config
Config file path, format is specific to the SLAM implementation in use.
Definition: t_tracking.h:651
bool write_csvs
Whether to enable CSV writers from the start for later analysis.
Definition: t_tracking.h:657
bool timing_stat
Enable timing metric in external system.
Definition: t_tracking.h:659
bool slam_ui
Whether to open the external UI of the external SLAM system.
Definition: t_tracking.h:653
Stereo camera calibration data to be given to trackers.
Definition: t_tracking.h:248
double camera_essential[3][3]
Essential matrix.
Definition: t_tracking.h:261
double camera_translation[3]
Translation from first to second in the stereo pair.
Definition: t_tracking.h:256
struct t_camera_calibration view[2]
Calibration of individual views/sensor.
Definition: t_tracking.h:253
double camera_rotation[3][3]
Rotation matrix from first to second in the stereo pair.
Definition: t_tracking.h:258
double camera_fundamental[3][3]
Fundamental matrix.
Definition: t_tracking.h:263
struct xrt_reference reference
Ref counting.
Definition: t_tracking.h:250
A 3 element colour with floating point channels.
Definition: xrt_defines.h:384
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:560
A base class for reference counted objects.
Definition: xrt_defines.h:96
Image size.
Definition: xrt_defines.h:409
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.