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 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
namespace	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

PSVR tracker code.

Author: Jakob Bornecrantz jakob.nosp@m.@col.nosp@m.labor.nosp@m.a.co.nosp@m.m; Pete Black pete..nosp@m.blac.nosp@m.k@col.nosp@m.labo.nosp@m.ra.co.nosp@m.m

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.

Data Structures

Namespaces

Macros

Typedefs

Enumerations

Functions