Monado OpenXR Runtime
t_tracker_psvr.cpp File Reference

PSVR tracker code. More...

#include "xrt/xrt_tracking.h"
#include "tracking/t_tracking.h"
#include "tracking/t_calibration_opencv.hpp"
#include "tracking/t_helper_debug_sink.hpp"
#include "util/u_misc.h"
#include "util/u_debug.h"
#include "util/u_frame.h"
#include "util/u_format.h"
#include "util/u_var.h"
#include "util/u_logging.h"
#include "math/m_mathinclude.h"
#include "math/m_api.h"
#include "math/m_permutation.h"
#include "math/m_imu_3dof.h"
#include "os/os_threading.h"
#include <stdio.h>
#include <assert.h>
#include <pthread.h>
#include <Hungarian.hpp>
#include <Eigen/Eigen>
#include <opencv2/opencv.hpp>
#include <inttypes.h>
Include dependency graph for t_tracker_psvr.cpp:

Data Structures

struct  xrt::auxiliary::tracking::psvr::blob_point
 
struct  xrt::auxiliary::tracking::psvr::View
 
struct  xrt::auxiliary::tracking::psvr::model_vertex
 
struct  xrt::auxiliary::tracking::psvr::match_data
 
struct  xrt::auxiliary::tracking::psvr::match_model
 
class  xrt::auxiliary::tracking::psvr::TrackerPSVR
 Main PSVR tracking class. More...
 
struct  xrt::auxiliary::tracking::psvr::close_pair
 
struct  xrt::auxiliary::tracking::psvr::proximity_data
 
struct  xrt::auxiliary::tracking::psvr::Helper
 
struct  xrt::auxiliary::tracking::psvr::blob_data
 

Namespaces

 xrt::auxiliary::tracking::psvr
 Namespace for PSVR tracking implementation.
 

Macros

#define PSVR_TRACE(...)   U_LOG_IFL_T(t.log_level, __VA_ARGS__)
 
#define PSVR_DEBUG(...)   U_LOG_IFL_D(t.log_level, __VA_ARGS__)
 
#define PSVR_INFO(...)   U_LOG_IFL_I(t.log_level, __VA_ARGS__)
 
#define PSVR_WARN(...)   U_LOG_IFL_W(t.log_level, __VA_ARGS__)
 
#define PSVR_ERROR(...)   U_LOG_IFL_E(t.log_level, __VA_ARGS__)
 
#define PSVR_NUM_LEDS   7
 How many LEDs in the tracked configuration. More...
 
#define PSVR_OPTICAL_SOLVE_THRESH   5
 How many LEDs do we need to do an optical solve/correction. More...
 
#define PSVR_DISAMBIG_REJECT_DIST   0.02f
 If potential match vertex is further than this distance from the measurement, reject the match - do not set too low. More...
 
#define PSVR_DISAMBIG_REJECT_ANG   0.7f
 If potential match vertex is further than this distance from the measurement, reject the match - do not set too low. More...
 
#define PSVR_SEARCH_RADIUS   0.043f
 Cutoff distance for keeping the id for a blob from one frame to the next. More...
 
#define PSVR_MAX_BAD_CORR   10
 
#define PSVR_BAD_CORRECTION_THRESH   0.1f
 
#define PSVR_CORRECTION_THRESH   0.05f
 
#define PSVR_FAST_CORRECTION   0.05f
 We will 'drift' our imu-solved rotation towards our optically solved correction to avoid jumps. More...
 
#define PSVR_SLOW_CORRECTION   0.005f
 We will 'drift' our imu-solved rotation towards our optically solved correction to avoid jumps. More...
 
#define PSVR_BLOB_PROCESS_NOISE   0.1f
 
#define PSVR_BLOB_MEASUREMENT_NOISE   1.0f
 
#define PSVR_POSE_PROCESS_NOISE   0.5f
 
#define PSVR_POSE_MEASUREMENT_NOISE   100.0f
 Our measurements are quite noisy so we need to smooth heavily. More...
 
#define PSVR_OUTLIER_THRESH   0.17f
 
#define PSVR_MERGE_THRESH   0.06f
 
#define PSVR_HOLD_THRESH   0.086f
 hold the previously recognised configuration unless we depart significantly More...
 

Typedefs

typedef enum xrt::auxiliary::tracking::psvr::blob_type xrt::auxiliary::tracking::psvr::blob_type_t
 
typedef struct xrt::auxiliary::tracking::psvr::blob_point xrt::auxiliary::tracking::psvr::blob_point_t
 
typedef enum xrt::auxiliary::tracking::psvr::led_tag xrt::auxiliary::tracking::psvr::led_tag_t
 
typedef struct xrt::auxiliary::tracking::psvr::model_vertex xrt::auxiliary::tracking::psvr::model_vertex_t
 
typedef struct xrt::auxiliary::tracking::psvr::match_data xrt::auxiliary::tracking::psvr::match_data_t
 
typedef struct xrt::auxiliary::tracking::psvr::match_model xrt::auxiliary::tracking::psvr::match_model_t
 
typedef struct xrt::auxiliary::tracking::psvr::proximity_data xrt::auxiliary::tracking::psvr::proximity_data_t
 
typedef struct xrt::auxiliary::tracking::psvr::blob_data xrt::auxiliary::tracking::psvr::blob_data_t
 

Enumerations

enum  blob_type { BLOB_TYPE_UNKNOWN , BLOB_TYPE_SIDE , BLOB_TYPE_FRONT , BLOB_TYPE_REAR }
 
enum  led_tag {
  TAG_TL , TAG_TR , TAG_C , TAG_BL ,
  TAG_BR , TAG_SL , TAG_SR
}
 

Functions

static float xrt::auxiliary::tracking::psvr::dist_3d (Eigen::Vector4f a, Eigen::Vector4f b)
 
static float xrt::auxiliary::tracking::psvr::dist_3d_cv (const cv::Point3f &a, const cv::Point3f &b)
 
static void xrt::auxiliary::tracking::psvr::init_filter (cv::KalmanFilter &kf, float process_cov, float meas_cov, float dt)
 
static void xrt::auxiliary::tracking::psvr::filter_predict (std::vector< match_data_t > *pose, cv::KalmanFilter *filters, float dt)
 
static void xrt::auxiliary::tracking::psvr::filter_update (std::vector< match_data_t > *pose, cv::KalmanFilter *filters, float dt)
 
static void xrt::auxiliary::tracking::psvr::pose_filter_predict (Eigen::Vector4f *pose, cv::KalmanFilter *filter, float dt)
 
static void xrt::auxiliary::tracking::psvr::pose_filter_update (Eigen::Vector4f *position, cv::KalmanFilter *filter, float dt)
 
static bool xrt::auxiliary::tracking::psvr::match_possible (match_model_t *match)
 
static void xrt::auxiliary::tracking::psvr::verts_to_measurement (std::vector< blob_point_t > *meas_data, std::vector< match_data_t > *match_vertices)
 
static float xrt::auxiliary::tracking::psvr::last_diff (TrackerPSVR &t, std::vector< match_data_t > *meas_pose, std::vector< match_data_t > *last_pose)
 
static void xrt::auxiliary::tracking::psvr::remove_outliers (std::vector< blob_point_t > *orig_points, std::vector< blob_point_t > *pruned_points, float outlier_thresh)
 
static void xrt::auxiliary::tracking::psvr::merge_close_points (std::vector< blob_point_t > *orig_points, std::vector< blob_point_t > *merged_points, float merge_thresh)
 
static void xrt::auxiliary::tracking::psvr::match_triangles (Eigen::Matrix4f *t1_mat, Eigen::Matrix4f *t1_to_t2_mat, const Eigen::Vector4f &t1_a, const Eigen::Vector4f &t1_b, const Eigen::Vector4f &t1_c, const Eigen::Vector4f &t2_a, const Eigen::Vector4f &t2_b, const Eigen::Vector4f &t2_c)
 
static Eigen::Matrix4f xrt::auxiliary::tracking::psvr::solve_for_measurement (TrackerPSVR *t, std::vector< match_data_t > *measurement, std::vector< match_data_t > *solved)
 
static Eigen::Matrix4f xrt::auxiliary::tracking::psvr::solve_with_imu (TrackerPSVR &t, std::vector< match_data_t > *measurements, std::vector< match_data_t > *match_measurements, std::vector< match_data_t > *solved, float search_radius)
 
static Eigen::Matrix4f xrt::auxiliary::tracking::psvr::disambiguate (TrackerPSVR &t, std::vector< match_data_t > *measured_points, std::vector< match_data_t > *last_measurement, std::vector< match_data_t > *solved, uint32_t frame_no)
 
static void xrt::auxiliary::tracking::psvr::create_model (TrackerPSVR &t)
 
static void xrt::auxiliary::tracking::psvr::create_match_list (TrackerPSVR &t)
 
static void xrt::auxiliary::tracking::psvr::do_view (TrackerPSVR &t, View &view, cv::Mat &grey, cv::Mat &rgb)
 
static void xrt::auxiliary::tracking::psvr::sample_line (cv::Mat &src, const cv::Point2i &start, const cv::Point2i &end, int *inside_length)
 
static void xrt::auxiliary::tracking::psvr::blob_intersections (cv::Mat &src, cv::KeyPoint *kp, struct blob_data *bd)
 
static void xrt::auxiliary::tracking::psvr::tag_points (TrackerPSVR &t, std::vector< blob_data_t > *blob_datas)
 
static void xrt::auxiliary::tracking::psvr::process (TrackerPSVR &t, struct xrt_frame *xf)
 
static void xrt::auxiliary::tracking::psvr::run (TrackerPSVR &t)
 
static void xrt::auxiliary::tracking::psvr::get_pose (TrackerPSVR &t, timepoint_ns when_ns, struct xrt_space_relation *out_relation)
 
static void xrt::auxiliary::tracking::psvr::imu_data (TrackerPSVR &t, timepoint_ns timestamp_ns, struct xrt_tracking_sample *sample)
 
static void xrt::auxiliary::tracking::psvr::frame (TrackerPSVR &t, struct xrt_frame *xf)
 
static void xrt::auxiliary::tracking::psvr::break_apart (TrackerPSVR &t)
 
void t_psvr_push_imu (struct xrt_tracked_psvr *xtvr, timepoint_ns timestamp_ns, struct xrt_tracking_sample *sample)
 
void t_psvr_get_tracked_pose (struct xrt_tracked_psvr *xtvr, timepoint_ns when_ns, struct xrt_space_relation *out_relation)
 
void t_psvr_fake_destroy (struct xrt_tracked_psvr *xtvr)
 
void t_psvr_sink_push_frame (struct xrt_frame_sink *xsink, struct xrt_frame *xf)
 
void t_psvr_node_break_apart (struct xrt_frame_node *node)
 
void t_psvr_node_destroy (struct xrt_frame_node *node)
 
void * t_psvr_run (void *ptr)
 
int t_psvr_start (struct xrt_tracked_psvr *xtvr)
 
int t_psvr_create (struct xrt_frame_context *xfctx, struct t_stereo_camera_calibration *data, struct xrt_tracked_psvr **out_xtvr, struct xrt_frame_sink **out_sink)
 

Detailed Description

Macro Definition Documentation

◆ PSVR_DISAMBIG_REJECT_ANG

#define PSVR_DISAMBIG_REJECT_ANG   0.7f

If potential match vertex is further than this distance from the measurement, reject the match - do not set too low.

◆ PSVR_DISAMBIG_REJECT_DIST

#define PSVR_DISAMBIG_REJECT_DIST   0.02f

If potential match vertex is further than this distance from the measurement, reject the match - do not set too low.

◆ PSVR_FAST_CORRECTION

#define PSVR_FAST_CORRECTION   0.05f

We will 'drift' our imu-solved rotation towards our optically solved correction to avoid jumps.

◆ PSVR_HOLD_THRESH

#define PSVR_HOLD_THRESH   0.086f

hold the previously recognised configuration unless we depart significantly

◆ PSVR_NUM_LEDS

#define PSVR_NUM_LEDS   7

How many LEDs in the tracked configuration.

◆ PSVR_OPTICAL_SOLVE_THRESH

#define PSVR_OPTICAL_SOLVE_THRESH   5

How many LEDs do we need to do an optical solve/correction.

◆ PSVR_POSE_MEASUREMENT_NOISE

#define PSVR_POSE_MEASUREMENT_NOISE   100.0f

Our measurements are quite noisy so we need to smooth heavily.

◆ PSVR_SEARCH_RADIUS

#define PSVR_SEARCH_RADIUS   0.043f

Cutoff distance for keeping the id for a blob from one frame to the next.

◆ PSVR_SLOW_CORRECTION

#define PSVR_SLOW_CORRECTION   0.005f

We will 'drift' our imu-solved rotation towards our optically solved correction to avoid jumps.