xrt::auxiliary::tracking::slam Namespace Reference

Namespace for the interface to the external SLAM tracking system. More...

Data Structures
class	CSVWriter
	Writes a CSV file for a particular row type. More...
struct	feature_count_sample
struct	FeaturesWriter
	Writes feature information specific to a particular estimated pose. More...
struct	TimingWriter
	Writes timestamps measured when estimating a new pose by the SLAM system. More...
struct	TrackerSlam
	Main implementation of xrt_tracked_slam. More...
struct	TrajectoryWriter
	Writes poses and their timestamps to a CSV file. More...

Typedefs
using	Trajectory = map< timepoint_ns, xrt_pose >
using	timing_sample = vector< timepoint_ns >

Functions
ostream &	operator<< (ostream &os, const xrt_pose_sample &s)
ostream &	operator<< (ostream &os, const timing_sample &timestamps)
ostream &	operator<< (ostream &os, const feature_count_sample &s)
static void	timing_ui_setup (TrackerSlam &t)
static vector< timepoint_ns >	timing_ui_push (TrackerSlam &t, const vit_pose_t *pose, int64_t ts)
	Updates timing UI with info from a computed pose and returns that info.
static void	features_ui_setup (TrackerSlam &t)
static vector< int >	features_ui_push (TrackerSlam &t, const vit_pose_t *pose, int64_t ts)
static xrt_pose	get_gt_pose_at (const Trajectory &gt, timepoint_ns ts)
	Gets an interpolated groundtruth pose (if available) at a specified timestamp.
static struct xrt_pose	xr2gt_pose (const xrt_pose &gt_origin, const xrt_pose &xr_pose)
	Converts a pose from the tracker to ground truth.
static struct xrt_pose	gt2xr_pose (const xrt_pose &gt_origin, const xrt_pose &gt_pose)
	The inverse of xr2gt_pose.
static void	gt_ui_setup (TrackerSlam &t)
static void	gt_ui_push (TrackerSlam &t, timepoint_ns ts, xrt_pose tracked_pose)
static bool	flush_poses (TrackerSlam &t)
	Dequeue all tracked poses from the SLAM system and update prediction data with them.
static void	predict_pose (TrackerSlam &t, timepoint_ns when_ns, struct xrt_space_relation *out_relation)
	Return our best guess of the relation at time when_ns using all the data the tracker has.
static void	filter_pose (TrackerSlam &t, timepoint_ns when_ns, struct xrt_space_relation *out_relation)
	Various filters to remove noise from the predicted trajectory.
static void	setup_ui (TrackerSlam &t)
static void	add_camera_calibration (const TrackerSlam &t, const t_slam_camera_calibration *calib, uint32_t cam_index)
static void	add_imu_calibration (const TrackerSlam &t, const t_slam_imu_calibration *imu_calib)
static void	send_calibration (const TrackerSlam &t, const t_slam_calibration &c)

Variables
constexpr int	UI_TIMING_POSE_COUNT = 192
constexpr int	UI_FEATURES_POSE_COUNT = 192
constexpr int	UI_GTDIFF_POSE_COUNT = 192

Detailed Description

Namespace for the interface to the external SLAM tracking system.

See also

t_slam_tracker_config

Function Documentation

filter_pose()

static void xrt::auxiliary::tracking::slam::filter_pose ( TrackerSlam &  t, timepoint_ns  when_ns, struct xrt_space_relation *  out_relation  )

static

Various filters to remove noise from the predicted trajectory.

Todo:

Implement the quaternion averaging with a m_ff_vec4_f32 and normalization. Although it would be best to have a way of generalizing types before so as to not have redundant copies of ff logic.

References xrt::auxiliary::tracking::slam::TrackerSlam::alpha, xrt::auxiliary::tracking::slam::TrackerSlam::filter, xrt::auxiliary::tracking::slam::TrackerSlam::last, m_ff_vec3_f32_filter(), m_ff_vec3_f32_push(), m_space_relation_interpolate(), xrt::auxiliary::tracking::slam::TrackerSlam::pos_oe, xrt::auxiliary::tracking::slam::TrackerSlam::rot_ff, xrt::auxiliary::tracking::slam::TrackerSlam::rot_oe, xrt::auxiliary::tracking::slam::TrackerSlam::target, U_TIME_1MS_IN_NS, xrt::auxiliary::tracking::slam::TrackerSlam::use_exponential_smoothing_filter, and xrt::auxiliary::tracking::slam::TrackerSlam::window.

Referenced by t_slam_get_tracked_pose().

flush_poses()

static bool xrt::auxiliary::tracking::slam::flush_poses ( TrackerSlam &  t )

static

Dequeue all tracked poses from the SLAM system and update prediction data with them.

References xrt::auxiliary::tracking::slam::TrackerSlam::dbg_pred_counter, xrt::auxiliary::tracking::slam::TrackerSlam::dbg_pred_every, xrt::auxiliary::tracking::slam::TrackerSlam::Features::enabled, xrt::auxiliary::tracking::slam::TrackerSlam::euroc_recorder, xrt_slam_sinks::gt, xrt::auxiliary::tracking::slam::TrackerSlam::slam_features_writer, xrt::auxiliary::tracking::slam::TrackerSlam::slam_rels, xrt::auxiliary::tracking::slam::TrackerSlam::slam_times_writer, xrt::auxiliary::tracking::slam::TrackerSlam::slam_traj_writer, time_ns_to_s(), timing_ui_push(), xrt::auxiliary::tracking::slam::TrackerSlam::tracker, and xrt::auxiliary::tracking::slam::TrackerSlam::vit.

Referenced by receive_frame(), and t_slam_get_tracked_pose().

get_gt_pose_at()

static xrt_pose xrt::auxiliary::tracking::slam::get_gt_pose_at ( const Trajectory &  gt, timepoint_ns  ts  )

static

Gets an interpolated groundtruth pose (if available) at a specified timestamp.

Referenced by t_slam_get_tracked_pose().

gt2xr_pose()

static struct xrt_pose xrt::auxiliary::tracking::slam::gt2xr_pose ( const xrt_pose &  gt_origin, const xrt_pose &  gt_pose  )

static

The inverse of xr2gt_pose.

Referenced by t_slam_get_tracked_pose().

predict_pose()

static void xrt::auxiliary::tracking::slam::predict_pose ( TrackerSlam &  t, timepoint_ns  when_ns, struct xrt_space_relation *  out_relation  )

static

Return our best guess of the relation at time when_ns using all the data the tracker has.

References xrt::auxiliary::tracking::slam::TrackerSlam::accel_ff, xrt::auxiliary::tracking::slam::TrackerSlam::gravity_correction, xrt::auxiliary::tracking::slam::TrackerSlam::gyro_ff, xrt::auxiliary::tracking::slam::TrackerSlam::last_imu_ts, xrt::auxiliary::tracking::slam::TrackerSlam::lock_ff, m_ff_vec3_f32_filter(), m_predict_relation(), xrt::auxiliary::tracking::slam::TrackerSlam::pred_type, SLAM_PRED_ACCEL_GYRO, SLAM_PRED_DEAD_RECKONING, SLAM_PRED_GYRO, SLAM_PRED_NONE, SLAM_PRED_POSE_ONLY, xrt::auxiliary::tracking::slam::TrackerSlam::slam_rels, t_apply_dead_reckoning(), and time_ns_to_s().

Referenced by t_slam_get_tracked_pose().

timing_ui_push()

static vector< timepoint_ns > xrt::auxiliary::tracking::slam::timing_ui_push ( TrackerSlam &  t, const vit_pose_t *  pose, int64_t  ts  )

static

Updates timing UI with info from a computed pose and returns that info.

References xrt::auxiliary::tracking::slam::TrackerSlam::dur_ms, xrt::auxiliary::tracking::slam::TrackerSlam::enabled, xrt::auxiliary::tracking::slam::TrackerSlam::end_ts_idx, xrt::auxiliary::tracking::slam::TrackerSlam::idx, os_monotonic_get_ns(), u_var_timing::reference_timing, xrt::auxiliary::tracking::slam::TrackerSlam::start_ts_idx, xrt::auxiliary::tracking::slam::TrackerSlam::timing, U_TIME_1MS_IN_NS, xrt::auxiliary::tracking::slam::TrackerSlam::ui, and xrt::auxiliary::tracking::slam::TrackerSlam::vit.

Referenced by flush_poses().

xr2gt_pose()

static struct xrt_pose xrt::auxiliary::tracking::slam::xr2gt_pose ( const xrt_pose &  gt_origin, const xrt_pose &  xr_pose  )

static

Converts a pose from the tracker to ground truth.

Todo:

Right now this is hardcoded for Basalt and the EuRoC vicon datasets groundtruth and ignores orientation. Applies a fixed transformation so that the tracked and groundtruth trajectories origins and general motion match. The usual way of evaluating trajectory errors in SLAM requires to first align the trajectories through a non-linear optimization (e.g. gauss newton) so that they are as similar as possible. For this you need the entire tracked trajectory to be known beforehand, which makes it not suitable for reporting an error metric in realtime. See this 2-page paper for more info on trajectory alignment: https://ylatif.github.io/movingsensors/cameraReady/paper07.pdf