SLAM tracking code. More...

#include "xrt/xrt_config_have.h"
#include "xrt/xrt_defines.h"
#include "xrt/xrt_tracking.h"
#include "xrt/xrt_frameserver.h"
#include "util/u_debug.h"
#include "util/u_logging.h"
#include "util/u_misc.h"
#include "util/u_sink.h"
#include "util/u_var.h"
#include "util/u_trace_marker.h"
#include "os/os_threading.h"
#include "math/m_api.h"
#include "math/m_filter_fifo.h"
#include "math/m_filter_one_euro.h"
#include "math/m_predict.h"
#include "math/m_relation_history.h"
#include "math/m_space.h"
#include "math/m_vec3.h"
#include "tracking/t_euroc_recorder.h"
#include "tracking/t_openvr_tracker.h"
#include "tracking/t_tracking.h"
#include "tracking/t_vit_loader.h"
#include "tracking/t_dead_reckoning.h"
#include "vit/vit_interface.h"
#include <opencv2/core/mat.hpp>
#include <opencv2/core/version.hpp>
#include <deque>
#include <filesystem>
#include <fstream>
#include <iomanip>
#include <map>
#include <mutex>
#include <string>
#include <vector>

Include dependency graph for t_tracker_slam.cpp:

Data Structures
struct	xrt::auxiliary::tracking::slam::feature_count_sample

class	xrt::auxiliary::tracking::slam::CSVWriter< RowType >
	Writes a CSV file for a particular row type. More...

struct	xrt::auxiliary::tracking::slam::TrajectoryWriter
	Writes poses and their timestamps to a CSV file. More...

struct	xrt::auxiliary::tracking::slam::TimingWriter
	Writes timestamps measured when estimating a new pose by the SLAM system. More...

struct	xrt::auxiliary::tracking::slam::FeaturesWriter
	Writes feature information specific to a particular estimated pose. More...

struct	xrt::auxiliary::tracking::slam::TrackerSlam
	Main implementation of xrt_tracked_slam. More...

struct	xrt::auxiliary::tracking::slam::TrackerSlam::Features
	Tracker feature tracking info. More...

struct	xrt::auxiliary::tracking::slam::TrackerSlam::Features::FeatureCounter

Namespaces
namespace	xrt::auxiliary::tracking::slam
	Namespace for the interface to the external SLAM tracking system.

Macros
#define	PREFERRED_VIT_SYSTEM_LIBRARY "libbasalt.so"

#define	SLAM_TRACE(...) U_LOG_IFL_T(t.log_level, __VA_ARGS__)

#define	SLAM_DEBUG(...) U_LOG_IFL_D(t.log_level, __VA_ARGS__)

#define	SLAM_INFO(...) U_LOG_IFL_I(t.log_level, __VA_ARGS__)

#define	SLAM_WARN(...) U_LOG_IFL_W(t.log_level, __VA_ARGS__)

#define	SLAM_ERROR(...) U_LOG_IFL_E(t.log_level, __VA_ARGS__)

#define	SLAM_ASSERT(predicate, ...)

#define	SLAM_ASSERT_(predicate) SLAM_ASSERT(predicate, "Assertion failed " #predicate)

#define	SLAM_DASSERT(predicate, ...) SLAM_ASSERT(predicate, __VA_ARGS__)

#define	SLAM_DASSERT_(predicate) SLAM_ASSERT_(predicate)

#define	DEFINE_RECEIVE_CAM(cam_id)

Typedefs
using	xrt::auxiliary::tracking::slam::Trajectory = map< timepoint_ns, xrt_pose >

using	xrt::auxiliary::tracking::slam::timing_sample = vector< timepoint_ns >

Functions
ostream &	xrt::auxiliary::tracking::slam::operator<< (ostream &os, const xrt_pose_sample &s)

ostream &	xrt::auxiliary::tracking::slam::operator<< (ostream &os, const timing_sample &timestamps)

ostream &	xrt::auxiliary::tracking::slam::operator<< (ostream &os, const feature_count_sample &s)

static void	xrt::auxiliary::tracking::slam::timing_ui_setup (TrackerSlam &t)

static vector< timepoint_ns >	xrt::auxiliary::tracking::slam::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	xrt::auxiliary::tracking::slam::features_ui_setup (TrackerSlam &t)

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

static xrt_pose	xrt::auxiliary::tracking::slam::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	xrt::auxiliary::tracking::slam::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	xrt::auxiliary::tracking::slam::gt2xr_pose (const xrt_pose &gt_origin, const xrt_pose &gt_pose)
	The inverse of xr2gt_pose.

static void	xrt::auxiliary::tracking::slam::gt_ui_setup (TrackerSlam &t)

static void	xrt::auxiliary::tracking::slam::gt_ui_push (TrackerSlam &t, timepoint_ns ts, xrt_pose tracked_pose)

static bool	xrt::auxiliary::tracking::slam::flush_poses (TrackerSlam &t)
	Dequeue all tracked poses from the SLAM system and update prediction data with them.

static void	xrt::auxiliary::tracking::slam::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	xrt::auxiliary::tracking::slam::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	xrt::auxiliary::tracking::slam::setup_ui (TrackerSlam &t)

static void	xrt::auxiliary::tracking::slam::add_camera_calibration (const TrackerSlam &t, const t_slam_camera_calibration *calib, uint32_t cam_index)

static void	xrt::auxiliary::tracking::slam::add_imu_calibration (const TrackerSlam &t, const t_slam_imu_calibration *imu_calib)

static void	xrt::auxiliary::tracking::slam::send_calibration (const TrackerSlam &t, const t_slam_calibration &c)

void	t_slam_get_tracked_pose (struct xrt_tracked_slam xts, timepoint_ns when_ns, struct xrt_space_relation out_relation)
	Get a filtered prediction from the SLAM tracked poses.

void	t_slam_gt_sink_push (struct xrt_pose_sink sink, xrt_pose_sample sample)
	Receive and register ground truth to use for trajectory error metrics.

void	t_slam_hand_mask_sink_push (struct xrt_hand_masks_sink sink, struct xrt_hand_masks_sample hand_masks)
	Receive and register masks to use in the next image.

void	t_slam_receive_imu (struct xrt_imu_sink sink, struct xrt_imu_sample s)
	Receive and send IMU samples to the external SLAM system.

static void	receive_frame (TrackerSlam &t, struct xrt_frame *frame, uint32_t cam_index)
	Push the frame to the external SLAM system.

void	t_slam_receive_cam0 (struct xrt_frame_sink sink, struct xrt_frame frame)

void	t_slam_receive_cam1 (struct xrt_frame_sink sink, struct xrt_frame frame)

void	t_slam_receive_cam2 (struct xrt_frame_sink sink, struct xrt_frame frame)

void	t_slam_receive_cam3 (struct xrt_frame_sink sink, struct xrt_frame frame)

void	t_slam_receive_cam4 (struct xrt_frame_sink sink, struct xrt_frame frame)

void	t_slam_node_break_apart (struct xrt_frame_node *node)

void	t_slam_node_destroy (struct xrt_frame_node *node)

int	t_slam_start (struct xrt_tracked_slam *xts)

void	t_slam_fill_default_config (struct t_slam_tracker_config *config)
	Fills in a t_slam_tracker_config with default values.

int	t_slam_create (struct xrt_frame_context xfctx, struct t_slam_tracker_config config, struct xrt_tracked_slam out_xts, struct xrt_slam_sinks out_sink)

Variables
constexpr int	xrt::auxiliary::tracking::slam::UI_TIMING_POSE_COUNT = 192

constexpr int	xrt::auxiliary::tracking::slam::UI_FEATURES_POSE_COUNT = 192

constexpr int	xrt::auxiliary::tracking::slam::UI_GTDIFF_POSE_COUNT = 192

void(*	t_slam_receive_cam [XRT_TRACKING_MAX_SLAM_CAMS])(xrt_frame_sink , xrt_frame )
	Define a function for each XRT_TRACKING_MAX_SLAM_CAMS and reference it in this array.

Detailed Description

SLAM tracking code.

Author: Mateo de Mayo mateo.nosp@m..dem.nosp@m.ayo@c.nosp@m.olla.nosp@m.bora..nosp@m.com

Macro Definition Documentation

◆ DEFINE_RECEIVE_CAM

#define DEFINE_RECEIVE_CAM ( cam_id )

Value:

        extern "C" void t_slam_receive_cam##cam_id(struct xrt_frame_sink *sink, struct xrt_frame *frame)               \
        {                                                                                                              \
                auto &t = *container_of(sink, TrackerSlam, cam_sinks[cam_id]);                                         \
                receive_frame(t, frame, cam_id);                                                                       \
                u_sink_debug_push_frame(&t.ui_sink[cam_id], frame);                                                    \
                xrt_sink_push_frame(t.euroc_recorder->cams[cam_id], frame);                                            \
        }

◆ PREFERRED_VIT_SYSTEM_LIBRARY

#define PREFERRED_VIT_SYSTEM_LIBRARY "libbasalt.so"

Todo:: Get preferred system from systems found at build time

◆ SLAM_ASSERT

#define SLAM_ASSERT	(	predicate,
		...
	)

Value:

        do {                                                                                                           \
                bool p = predicate;                                                                                    \
                if (!p) {                                                                                              \
                        U_LOG(U_LOGGING_ERROR, __VA_ARGS__);                                                           \
                        assert(false && "SLAM_ASSERT failed: " #predicate);                                            \
                        exit(EXIT_FAILURE);                                                                            \
                }                                                                                                      \
        } while (false);

Function Documentation

◆ receive_frame()

static void receive_frame	(	TrackerSlam &	t,
		struct xrt_frame *	frame,
		uint32_t	cam_index
	)

static

Push the frame to the external SLAM system.

References xrt::auxiliary::tracking::slam::TrackerSlam::cam_count, xrt::auxiliary::tracking::slam::flush_poses(), xrt::auxiliary::tracking::slam::TrackerSlam::last_cam_ts, xrt::auxiliary::tracking::slam::TrackerSlam::last_hand_masks, xrt::auxiliary::tracking::slam::TrackerSlam::last_hand_masks_mutex, os_monotonic_get_ns(), xrt::auxiliary::tracking::slam::TrackerSlam::submit, xrt::auxiliary::tracking::slam::TrackerSlam::tracker, and xrt::auxiliary::tracking::slam::TrackerSlam::vit.

◆ t_slam_get_tracked_pose()

void t_slam_get_tracked_pose	(	struct xrt_tracked_slam *	xts,
		timepoint_ns	when_ns,
		struct xrt_space_relation *	out_relation
	)

Get a filtered prediction from the SLAM tracked poses.

Todo:: This should not be cached, the same timestamp can be requested at a later time on the frame for a better prediction.

◆ t_slam_gt_sink_push()

void t_slam_gt_sink_push	(	struct xrt_pose_sink *	sink,
		xrt_pose_sample *	sample
	)

Receive and register ground truth to use for trajectory error metrics.

References container_of, xrt::auxiliary::tracking::slam::TrackerSlam::euroc_recorder, xrt::auxiliary::tracking::slam::TrackerSlam::gt, xrt_slam_sinks::gt, xrt::auxiliary::tracking::slam::TrackerSlam::origin, and xrt::auxiliary::tracking::slam::TrackerSlam::trajectory.

◆ t_slam_hand_mask_sink_push()

void t_slam_hand_mask_sink_push	(	struct xrt_hand_masks_sink *	sink,
		struct xrt_hand_masks_sample *	hand_masks
	)

Receive and register masks to use in the next image.

References container_of, xrt::auxiliary::tracking::slam::TrackerSlam::last_hand_masks, and xrt::auxiliary::tracking::slam::TrackerSlam::last_hand_masks_mutex.

◆ t_slam_receive_imu()

void t_slam_receive_imu	(	struct xrt_imu_sink *	sink,
		struct xrt_imu_sample *	s
	)

Receive and send IMU samples to the external SLAM system.

Todo:: There are many conversions like these between xrt and slam_tracker.hpp types. Implement a casting mechanism to avoid copies.

Variable Documentation

◆ t_slam_receive_cam

void(* t_slam_receive_cam[XRT_TRACKING_MAX_SLAM_CAMS])(xrt_frame_sink , xrt_frame )	(	xrt_frame_sink *	,
		xrt_frame *
	)

Initial value:

= {
    t_slam_receive_cam0, 
    t_slam_receive_cam1, 
    t_slam_receive_cam2, 
    t_slam_receive_cam3, 
    t_slam_receive_cam4, 
}

Define a function for each XRT_TRACKING_MAX_SLAM_CAMS and reference it in this array.

Data Structures

Namespaces

Macros

Typedefs

Functions

Variables