RealSense device tracked with host-SLAM. More...

#include "xrt/xrt_device.h"
#include "xrt/xrt_frameserver.h"
#include "xrt/xrt_frame.h"
#include "xrt/xrt_prober.h"
#include "xrt/xrt_config_have.h"
#include "xrt/xrt_config_build.h"
#include "math/m_filter_fifo.h"
#include "math/m_space.h"
#include "os/os_time.h"
#include "util/u_device.h"
#include "util/u_logging.h"
#include "util/u_debug.h"
#include "util/u_var.h"
#include "util/u_sink.h"
#include "util/u_config_json.h"
#include "util/u_format.h"
#include "tracking/t_tracking.h"
#include "rs_driver.h"
#include <math.h>
#include <stdio.h>
#include <assert.h>
#include <librealsense2/rs.h>
#include <librealsense2/h/rs_pipeline.h>

Include dependency graph for rs_hdev.c:

Data Structures
struct	rs_hdev
	Host-SLAM tracked RealSense device (any RealSense device with camera and IMU streams). More...

struct	rs_source
	RealSense source of camera and IMU data. More...

Macros
#define	DEFAULT_STEREO true

#define	DEFAULT_XRT_VIDEO_FORMAT XRT_FORMAT_L8

#define	DEFAULT_VIDEO_FORMAT RS2_FORMAT_Y8

#define	DEFAULT_VIDEO_WIDTH 640

#define	DEFAULT_VIDEO_HEIGHT 360

#define	DEFAULT_VIDEO_FPS 30

#define	DEFAULT_VIDEO_CHANGE_EXPOSURE true

#define	DEFAULT_VIDEO_AUTOEXPOSURE false

#define	DEFAULT_VIDEO_EXPOSURE 6000

#define	DEFAULT_VIDEO_GAIN 127

#define	DEFAULT_GYRO_FPS 200

#define	DEFAULT_ACCEL_FPS 250

#define	DEFAULT_STREAM_TYPE RS2_STREAM_INFRARED

#define	DEFAULT_STREAM1_INDEX 1

#define	DEFAULT_STREAM2_INDEX 2

#define	RS_DEVICE_STR "Intel RealSense Host-SLAM"

#define	RS_SOURCE_STR "RealSense Source"

#define	RS_HOST_SLAM_TRACKER_STR "Host SLAM Tracker for RealSense"

#define	RS_TRACE(r, ...) U_LOG_IFL_T(r->log_level, __VA_ARGS__)

#define	RS_DEBUG(r, ...) U_LOG_IFL_D(r->log_level, __VA_ARGS__)

#define	RS_INFO(r, ...) U_LOG_IFL_I(r->log_level, __VA_ARGS__)

#define	RS_WARN(r, ...) U_LOG_IFL_W(r->log_level, __VA_ARGS__)

#define	RS_ERROR(r, ...) U_LOG_IFL_E(r->log_level, __VA_ARGS__)

#define	RS_ASSERT(predicate, ...)

#define	RS_ASSERT_(predicate) RS_ASSERT(predicate, "Assertion failed " #predicate)

#define	RS_DASSERT(predicate, ...) RS_ASSERT(predicate, __VA_ARGS__)

#define	RS_DASSERT_(predicate) RS_ASSERT_(predicate)

#define	DO(call, ...)
	Utility for realsense API calls that can produce errors. More...

#define	DO_(call)
	Alternative to DO() with no arguments. More...

#define	JSON_CONFIG_FIELD_NAME "config_realsense_hdev"

Functions
static void	receive_left_frame (struct xrt_frame_sink sink, struct xrt_frame )

static void	receive_right_frame (struct xrt_frame_sink sink, struct xrt_frame )

static void	receive_imu_sample (struct xrt_imu_sink sink, struct xrt_imu_sample )

static void	rs_source_node_break_apart (struct xrt_frame_node *)

static void	rs_source_node_destroy (struct xrt_frame_node *)

static bool	check_error (struct rs_source rs, rs2_error e, const char *file, int line)

static struct rs_hdev *	rs_hdev_from_xdev (struct xrt_device *xdev)

static XRT_MAYBE_UNUSED struct xrt_pose	rs_hdev_correct_pose_from_kimera (struct xrt_pose pose)
	Specific pose corrections for Kimera and the D455 camera. More...

static XRT_MAYBE_UNUSED struct xrt_pose	rs_hdev_correct_pose_from_basalt (struct xrt_pose pose)
	Specific pose corrections for Basalt and the D455 camera. More...

static xrt_result_t	rs_hdev_get_tracked_pose (struct xrt_device xdev, enum xrt_input_name name, int64_t at_timestamp_ns, struct xrt_space_relation out_relation)

static void	rs_hdev_destroy (struct xrt_device *xdev)

static void	json_int (struct rs_source rs, const cJSON json, const char field, int out)
	Helper function for loading an int field from a json container and printing useful messages along it. More...

static void	json_bool (struct rs_source rs, const cJSON json, const char field, bool out)
	Similar to json_int but for bools. More...

static void	json_rs2_format (struct rs_source rs, const cJSON json, const char field, rs2_format out_rformat, enum xrt_format *out_xformat)
	Similar to json_int but for a video rs2_format, also sets the equivalent xrt_format if any. More...

static void	json_rs2_stream (struct rs_source rs, const cJSON json, const char field, rs2_stream out_stream)
	Similar to json_int but for a rs2_stream type. More...

static void	rs_source_load_stream_options_from_json (struct rs_source *rs)

static bool	set_option_in_all_sensors (struct rs_source *rs, enum rs2_option option, float value)
	Set an option for all sensors. Return whether it was set for any. More...

static void	update_options (struct rs_source *rs)
	Submit changes to supported options to the device. More...

static void	rs_source_frame_destroy (struct xrt_frame *xf)

static timepoint_ns	get_frame_monotonic_ts_from (struct rs_source rs, rs2_frame frame, uint64_t unow_monotonic, uint64_t unow_realtime)

static timepoint_ns	get_frame_monotonic_ts (struct rs_source rs, rs2_frame frame)
	Convert device_ts to monotonic clock. More...

static void	rs2xrt_frame (struct rs_source rs, rs2_frame rframe, struct xrt_frame **out_xframe)

static void	handle_frameset (struct rs_source rs, rs2_frame frames)

static void	partial_imu_sample_push (struct rs_source *rs, timepoint_ns ts, struct xrt_vec3 vals, bool is_gyro)
	Decides when to submit the full IMU sample out of separate gyroscope/accelerometer samples. More...

static void	handle_gyro_frame (struct rs_source rs, rs2_frame frame)

static void	handle_accel_frame (struct rs_source rs, rs2_frame frame)

static void	check_global_time (struct rs_source rs, rs2_frame frame, rs2_stream stream_type)
	Checks that the timestamp domain of the realsense sample (the frame) is in global time or, at the very least, in another domain that we support. More...

static void	on_frame (rs2_frame frame, void ptr)

static void	rs_source_apply_changes_ctn_cb (void *ptr)

static struct rs_source *	rs_source_from_xfs (struct xrt_fs *xfs)

static bool	rs_source_enumerate_modes (struct xrt_fs xfs, struct xrt_fs_mode out_modes, uint32_t out_count)

static bool	rs_source_configure_capture (struct xrt_fs xfs, struct xrt_fs_capture_parameters cp)

static bool	rs_source_stream_stop (struct xrt_fs *xfs)

static bool	rs_source_is_running (struct xrt_fs *xfs)

static bool	rs_source_stream_start (struct xrt_fs xfs, struct xrt_frame_sink xs, enum xrt_fs_capture_type capture_type, uint32_t descriptor_index)

static bool	rs_source_slam_stream_start (struct xrt_fs xfs, struct xrt_slam_sinks sinks)

struct xrt_device *	rs_hdev_create (struct xrt_prober *xp, int device_idx)
	Create RealSense device tracked with host-SLAM (one with camera and IMU streams) More...

struct xrt_fs *	rs_source_create (struct xrt_frame_context *xfctx, int device_idx)
	Create and open the frame server for IMU/camera streaming. More...

Detailed Description

RealSense device tracked with host-SLAM.

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

Originally created and tried on the D455 model but should work in any RealSense device that has video and IMU streams.

Be aware that you need to properly set the SLAM_CONFIG file to match your camera specifics (stereo/mono, intrinsics, extrinsics, etc).

Macro Definition Documentation

◆ DO

#define DO	(	call,
		...
	)

Value:

call(__VA_ARGS__, &rs->rsc.error_status); \

check_error(rs, rs->rsc.error_status, __FILE__, __LINE__)

Utility for realsense API calls that can produce errors.

◆ DO_

#define DO_ ( call )

Value:

call(&rs->rsc.error_status); \

check_error(rs, rs->rsc.error_status, __FILE__, __LINE__)

Alternative to DO() with no arguments.

◆ RS_ASSERT

#define RS_ASSERT	(	predicate,
		...
	)

Value:

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

Function Documentation

◆ check_error()

static bool check_error	(	struct rs_source *	rs,
		rs2_error *	e,
		const char *	file,
		int	line
	)

static

Todo:: Unify check_error() and DO() usage thorough the driver.

◆ check_global_time()

static void check_global_time	(	struct rs_source *	rs,
		rs2_frame *	frame,
		rs2_stream	stream_type
	)

inlinestatic

Checks that the timestamp domain of the realsense sample (the frame) is in global time or, at the very least, in another domain that we support.

Note: We should be ensuring that we have the same timestamp domains in all sensors. But the user might have a newer kernel versions that is not supported by the RealSense DKMS package that allows GLOBAL_TIME for all sensors. From my experience and based on other users' reports, the only affected sensor without GLOBAL_TIME is the gyroscope, which is ~30ms off. See https://github.com/IntelRealSense/librealsense/issues/5710

References rs_source::accel_fps, DO, and rs_source::gyro_fps.

◆ get_frame_monotonic_ts()

static timepoint_ns get_frame_monotonic_ts	(	struct rs_source *	rs,
		rs2_frame *	frame
	)

inlinestatic

Convert device_ts to monotonic clock.

Assumes now_realtime and device_ts are in the same clock domain (GLOBAL_TIME).

References os_monotonic_get_ns(), and os_realtime_get_ns().

◆ json_bool()

static void json_bool	(	struct rs_source *	rs,
		const cJSON *	json,
		const char *	field,
		bool *	out
	)

static

Similar to json_int but for bools.

References u_json_get(), and u_json_get_bool().

◆ json_int()

static void json_int	(	struct rs_source *	rs,
		const cJSON *	json,
		const char *	field,
		int *	out
	)

static

Helper function for loading an int field from a json container and printing useful messages along it.

*out is expected to come preloaded with a default value.

References u_json_get(), and u_json_get_int().

◆ json_rs2_format()

static void json_rs2_format	(	struct rs_source *	rs,
		const cJSON *	json,
		const char *	field,
		rs2_format *	out_rformat,
		enum xrt_format *	out_xformat
	)

static

Similar to json_int but for a video rs2_format, also sets the equivalent xrt_format if any.

References u_json_get(), and u_json_get_int().

◆ json_rs2_stream()

static void json_rs2_stream	(	struct rs_source *	rs,
		const cJSON *	json,
		const char *	field,
		rs2_stream *	out_stream
	)

static

Similar to json_int but for a rs2_stream type.

References u_json_get(), and u_json_get_int().

◆ partial_imu_sample_push()

static void partial_imu_sample_push	(	struct rs_source *	rs,
		timepoint_ns	ts,
		struct xrt_vec3	vals,
		bool	is_gyro
	)

static

Decides when to submit the full IMU sample out of separate gyroscope/accelerometer samples.

References rs_source::accel, rs_source::gyro, rs_source::mutex, os_mutex::os_mutex_lock(), and rs_source::partial_imu_sample.

◆ receive_left_frame()

static void receive_left_frame	(	struct xrt_frame_sink *	sink,
		struct xrt_frame *	xf
	)

static

Todo:: Use one RS_LOG option for the entire driver

References container_of, and rs_source::left_sink.

◆ rs2xrt_frame()

static void rs2xrt_frame	(	struct rs_source *	rs,
		rs2_frame *	rframe,
		struct xrt_frame **	out_xframe
	)

static

<

Todo:: Use a stereo xrt_format

◆ rs_hdev_correct_pose_from_basalt()

static XRT_MAYBE_UNUSED struct xrt_pose rs_hdev_correct_pose_from_basalt ( struct xrt_pose pose )

inlinestatic

Specific pose corrections for Basalt and the D455 camera.

Todo:: Encode this transformation into constants

◆ rs_hdev_correct_pose_from_kimera()

static XRT_MAYBE_UNUSED struct xrt_pose rs_hdev_correct_pose_from_kimera ( struct xrt_pose pose )

inlinestatic

Specific pose corrections for Kimera and the D455 camera.

Todo:: Encode this transformation into constants

◆ rs_source_configure_capture()

static bool rs_source_configure_capture	(	struct xrt_fs *	xfs,
		struct xrt_fs_capture_parameters *	cp
	)

static

Todo:: implement

Referenced by rs_source_create().

◆ rs_source_create()

struct xrt_fs * rs_source_create	(	struct xrt_frame_context *	xfctx,
		int	device_idx
	)

Create and open the frame server for IMU/camera streaming.

Creates a RealSense SLAM source from the appropriate device_idx.

References xrt_fs::configure_capture, xrt_fs::enumerate_modes, rs_source::log_level, rs_source_configure_capture(), rs_source_enumerate_modes(), xrt_fs::stream_start, and U_TYPED_CALLOC.

◆ rs_source_enumerate_modes()

static bool rs_source_enumerate_modes	(	struct xrt_fs *	xfs,
		struct xrt_fs_mode **	out_modes,
		uint32_t *	out_count
	)

static

Todo:: only exposing the one stream configuration the user provided through the json configuration but we could show all possible stream setups.

Todo:: The stereo_format being NONE is incorrect but one that supports frames in different memory regions does not exist yet.

References container_of, and U_TYPED_ARRAY_CALLOC.

Referenced by rs_source_create().

◆ set_option_in_all_sensors()

static bool set_option_in_all_sensors	(	struct rs_source *	rs,
		enum rs2_option	option,
		float	value
	)

static

Set an option for all sensors. Return whether it was set for any.

References rs_container::device, DO, and rs_source::rsc.

Referenced by update_options().

◆ update_options()

static void update_options ( struct rs_source * rs )

static

Submit changes to supported options to the device.

References set_option_in_all_sensors(), rs_source::video_autoexposure, rs_source::video_exposure, and rs_source::video_gain.

Data Structures

Macros

Functions

Detailed Description

Macro Definition Documentation

◆ DO

◆ DO_

◆ RS_ASSERT

Function Documentation

◆ check_error()

◆ check_global_time()

◆ get_frame_monotonic_ts()

◆ json_bool()

◆ json_int()

◆ json_rs2_format()

◆ json_rs2_stream()

◆ partial_imu_sample_push()

◆ receive_left_frame()

◆ rs2xrt_frame()

◆ rs_hdev_correct_pose_from_basalt()

◆ rs_hdev_correct_pose_from_kimera()

◆ rs_source_configure_capture()

◆ rs_source_create()

◆ rs_source_enumerate_modes()

◆ set_option_in_all_sensors()

◆ update_options()