matrixfurry.com
The open source OpenXR runtime
1// Copyright 2020-2023, Collabora, Ltd.
2// Copyright 2020, Nova King.
3// SPDX-License-Identifier: BSL-1.0
4/*!
5 * @file
6 * @brief RealSense helper driver for in-device SLAM 6DOF tracking.
7 * @author Moshi Turner <moshiturner@protonmail.com>
8 * @author Nova King <technobaboo@gmail.com>
9 * @author Jakob Bornecrantz <jakob@collabora.com>
10 * @ingroup drv_realsense
11 */
12
13#include "xrt/xrt_defines.h"
14#include "xrt/xrt_device.h"
15#include "math/m_api.h"
16#include "math/m_space.h"
17#include "math/m_predict.h"
18#include "math/m_relation_history.h"
19
20#include "os/os_time.h"
21#include "os/os_threading.h"
22
23#include "util/u_time.h"
24#include "util/u_device.h"
25#include "util/u_logging.h"
26
27#include "util/u_json.h"
28#include "util/u_config_json.h"
29
30#include "rs_driver.h"
31
32#include <librealsense2/rs.h>
33#include <librealsense2/h/rs_pipeline.h>
34#include <librealsense2/h/rs_option.h>
35#include <librealsense2/h/rs_frame.h>
36
37#include <stdio.h>
38#include <assert.h>
39#include <stdlib.h>
40
41
42/*!
43 * Convenience macro to print out a pose, only used for debugging
44 */
45#define print_pose(msg, pose) \
46 U_LOG_E(msg " %f %f %f %f %f %f %f", pose.position.x, pose.position.y, pose.position.z, pose.orientation.x, \
47 pose.orientation.y, pose.orientation.z, pose.orientation.w)
48
49/*!
50 * Device-SLAM tracked RealSense device (T26X series).
51 *
52 * @implements xrt_device
53 */
54struct rs_ddev
55{
56 struct xrt_device base;
57
58 struct m_relation_history *relation_hist;
59
60 struct os_thread_helper oth;
61
62 bool enable_mapping;
63 bool enable_pose_jumping;
64 bool enable_relocalization;
65 bool enable_pose_prediction;
66 bool enable_pose_filtering; //!< Forward compatibility for when that 1-euro filter is working
67
68 struct rs_container rsc; //!< Container of realsense API related objects
69};
70
71
72/*!
73 * Helper to convert a xdev to a @ref rs_ddev.
74 */
75static inline struct rs_ddev *
76rs_ddev(struct xrt_device *xdev)
77{
78 return (struct rs_ddev *)xdev;
79}
80
81/*!
82 * Simple helper to check and print error messages.
83 */
84static int
85check_error(struct rs_ddev *rs, rs2_error *e)
86{
87 if (e == NULL) {
88 return 0;
89 }
90
91 U_LOG_E("rs_error was raised when calling %s(%s):", rs2_get_failed_function(e), rs2_get_failed_args(e));
92 U_LOG_E("%s", rs2_get_error_message(e));
93
94 return 1;
95}
96
97/*!
98 * Frees all RealSense resources.
99 */
100static void
101close_ddev(struct rs_ddev *rs)
102{
103 struct rs_container *rsc = &rs->rsc;
104 rs2_pipeline_stop(rsc->pipeline, NULL);
105 rs_container_cleanup(&rs->rsc);
106}
107
108#define CHECK_RS2() \
109 do { \
110 if (check_error(rs, e) != 0) { \
111 close_ddev(rs); \
112 return 1; \
113 } \
114 } while (0)
115
116
117/*!
118 * Create all RealSense resources needed for 6DOF tracking.
119 */
120static int
121create_ddev(struct rs_ddev *rs, int device_idx)
122{
123 assert(rs != NULL);
124 rs2_error *e = NULL;
125
126 struct rs_container *rsc = &rs->rsc;
127
128 rsc->context = rs2_create_context(RS2_API_VERSION, &e);
129 CHECK_RS2();
130
131 rsc->device_list = rs2_query_devices(rsc->context, &e);
132 CHECK_RS2();
133
134 rsc->pipeline = rs2_create_pipeline(rsc->context, &e);
135 CHECK_RS2();
136
137 rsc->config = rs2_create_config(&e);
138 CHECK_RS2();
139
140 // Set the pipeline to start specifically on the realsense device the prober selected
141 rsc->device_idx = device_idx;
142 rsc->device = rs2_create_device(rsc->device_list, rsc->device_idx, &e);
143 CHECK_RS2();
144
145 bool ddev_has_serial = rs2_supports_device_info(rsc->device, RS2_CAMERA_INFO_SERIAL_NUMBER, &e);
146 CHECK_RS2();
147
148 if (ddev_has_serial) {
149
150 const char *ddev_serial = rs2_get_device_info(rsc->device, RS2_CAMERA_INFO_SERIAL_NUMBER, &e);
151 CHECK_RS2();
152
153 rs2_config_enable_device(rsc->config, ddev_serial, &e);
154 CHECK_RS2();
155
156 } else {
157 U_LOG_W("Unexpected, the realsense device in use does not provide a serial number.");
158 }
159
160 rs2_delete_device(rsc->device);
161
162 rs2_config_enable_stream(rsc->config, //
163 RS2_STREAM_POSE, // Type
164 0, // Index
165 0, // Width
166 0, // Height
167 RS2_FORMAT_6DOF, // Format
168 200, // FPS
169 &e);
170 CHECK_RS2();
171
172 rsc->profile = rs2_config_resolve(rsc->config, rsc->pipeline, &e);
173 CHECK_RS2();
174
175 rsc->device = rs2_pipeline_profile_get_device(rsc->profile, &e);
176 CHECK_RS2();
177
178 rs2_sensor_list *sensors = rs2_query_sensors(rsc->device, &e);
179 CHECK_RS2();
180
181 //! @todo 0 index hardcoded, check device with RS2_EXTENSION_POSE_SENSOR or similar instead
182 rs2_sensor *sensor = rs2_create_sensor(sensors, 0, &e);
183 CHECK_RS2();
184
185 rs2_set_option((rs2_options *)sensor, RS2_OPTION_ENABLE_MAPPING, rs->enable_mapping ? 1.0f : 0.0f, &e);
186 CHECK_RS2();
187
188 if (rs->enable_mapping) {
189 // Neither of these options mean anything if mapping is off; in fact it
190 // errors out if we mess with these with mapping off
191 rs2_set_option((rs2_options *)sensor, RS2_OPTION_ENABLE_RELOCALIZATION,
192 rs->enable_relocalization ? 1.0f : 0.0f, &e);
193 CHECK_RS2();
194
195 rs2_set_option((rs2_options *)sensor, RS2_OPTION_ENABLE_POSE_JUMPING,
196 rs->enable_pose_jumping ? 1.0f : 0.0f, &e);
197 CHECK_RS2();
198 }
199
200 rsc->profile = rs2_pipeline_start_with_config(rsc->pipeline, rsc->config, &e);
201 CHECK_RS2();
202
203 rs2_delete_sensor(sensor);
204 rs2_delete_sensor_list(sensors);
205
206 return 0;
207}
208
209/*!
210 * Process a frame as 6DOF data, does not assume ownership of the frame.
211 */
212static void
213process_frame(struct rs_ddev *rs, rs2_frame *frame)
214{
215 rs2_error *e = NULL;
216 int ret = 0;
217
218 ret = rs2_is_frame_extendable_to(frame, RS2_EXTENSION_POSE_FRAME, &e);
219 if (check_error(rs, e) != 0 || ret == 0) {
220 return;
221 }
222
223 rs2_pose camera_pose;
224 rs2_pose_frame_get_pose_data(frame, &camera_pose, &e);
225 if (check_error(rs, e) != 0) {
226 return;
227 }
228
229#if 0
230 rs2_timestamp_domain domain = rs2_get_frame_timestamp_domain(frame, &e);
231 if (check_error(rs, e) != 0) {
232 return;
233 }
234#endif
235
236 double timestamp_milliseconds = rs2_get_frame_timestamp(frame, &e);
237 if (check_error(rs, e) != 0) {
238 return;
239 }
240
241 // Close enough
242 uint64_t now_real_ns = os_realtime_get_ns();
243 uint64_t now_monotonic_ns = os_monotonic_get_ns();
244 uint64_t timestamp_ns = (uint64_t)(timestamp_milliseconds * 1000.0 * 1000.0);
245
246 // How far in the past is it?
247 uint64_t diff_ns = now_real_ns - timestamp_ns;
248
249 // Adjust the timestamp to monotonic time.
250 timestamp_ns = now_monotonic_ns - diff_ns;
251
252 /*
253 * Transfer the data to the struct.
254 */
255
256 struct xrt_space_relation relation;
257
258 // Rotation/angular
259 relation.pose.orientation.x = camera_pose.rotation.x;
260 relation.pose.orientation.y = camera_pose.rotation.y;
261 relation.pose.orientation.z = camera_pose.rotation.z;
262 relation.pose.orientation.w = camera_pose.rotation.w;
263 relation.angular_velocity.x = camera_pose.angular_velocity.x;
264 relation.angular_velocity.y = camera_pose.angular_velocity.y;
265 relation.angular_velocity.z = camera_pose.angular_velocity.z;
266
267 // Position/linear
268 relation.pose.position.x = camera_pose.translation.x;
269 relation.pose.position.y = camera_pose.translation.y;
270 relation.pose.position.z = camera_pose.translation.z;
271 relation.linear_velocity.x = camera_pose.velocity.x;
272 relation.linear_velocity.y = camera_pose.velocity.y;
273 relation.linear_velocity.z = camera_pose.velocity.z;
274
275 // clang-format off
276 relation.relation_flags =
277 XRT_SPACE_RELATION_ORIENTATION_VALID_BIT |
278 XRT_SPACE_RELATION_POSITION_VALID_BIT |
279 XRT_SPACE_RELATION_LINEAR_VELOCITY_VALID_BIT |
280 XRT_SPACE_RELATION_ANGULAR_VELOCITY_VALID_BIT |
281 XRT_SPACE_RELATION_ORIENTATION_TRACKED_BIT |
282 XRT_SPACE_RELATION_POSITION_TRACKED_BIT;
283 // clang-format on
284
285 m_relation_history_push(rs->relation_hist, &relation, timestamp_ns);
286}
287
288static int
289update(struct rs_ddev *rs)
290{
291 rs2_frame *frames;
292 rs2_error *e = NULL;
293
294 frames = rs2_pipeline_wait_for_frames(rs->rsc.pipeline, RS2_DEFAULT_TIMEOUT, &e);
295 if (check_error(rs, e) != 0) {
296 return 1;
297 }
298
299 int num_frames = rs2_embedded_frames_count(frames, &e);
300 if (check_error(rs, e) != 0) {
301 return 1;
302 }
303
304 for (int i = 0; i < num_frames; i++) {
305 rs2_frame *frame = rs2_extract_frame(frames, i, &e);
306 if (check_error(rs, e) != 0) {
307 rs2_release_frame(frames);
308 return 1;
309 }
310
311 // Does not assume ownership of the frame.
312 process_frame(rs, frame);
313 rs2_release_frame(frame);
314
315 rs2_release_frame(frames);
316 }
317
318 return 0;
319}
320
321static void *
322rs_run_thread(void *ptr)
323{
324 struct rs_ddev *rs = (struct rs_ddev *)ptr;
325
326 os_thread_helper_lock(&rs->oth);
327
328 while (os_thread_helper_is_running_locked(&rs->oth)) {
329
330 os_thread_helper_unlock(&rs->oth);
331
332 int ret = update(rs);
333 if (ret < 0) {
334 return NULL;
335 }
336 }
337
338 return NULL;
339}
340
341static bool
342load_config(struct rs_ddev *rs)
343{
344 struct u_config_json config_json = {0};
345
346 u_config_json_open_or_create_main_file(&config_json);
347 if (!config_json.file_loaded) {
348 return false;
349 }
350
351 const cJSON *realsense_config_json = u_json_get(config_json.root, "config_realsense_ddev");
352 if (realsense_config_json == NULL) {
353 return false;
354 }
355
356 const cJSON *mapping = u_json_get(realsense_config_json, "enable_mapping");
357 const cJSON *pose_jumping = u_json_get(realsense_config_json, "enable_pose_jumping");
358 const cJSON *relocalization = u_json_get(realsense_config_json, "enable_relocalization");
359 const cJSON *pose_prediction = u_json_get(realsense_config_json, "enable_pose_prediction");
360 const cJSON *pose_filtering = u_json_get(realsense_config_json, "enable_pose_filtering");
361
362 // if json key isn't in the json, default to true. if it is in there, use json value
363 if (mapping != NULL) {
364 rs->enable_mapping = cJSON_IsTrue(mapping);
365 }
366 if (pose_jumping != NULL) {
367 rs->enable_pose_jumping = cJSON_IsTrue(pose_jumping);
368 }
369 if (relocalization != NULL) {
370 rs->enable_relocalization = cJSON_IsTrue(relocalization);
371 }
372 if (pose_prediction != NULL) {
373 rs->enable_pose_prediction = cJSON_IsTrue(pose_prediction);
374 }
375 if (pose_filtering != NULL) {
376 rs->enable_pose_filtering = cJSON_IsTrue(pose_filtering);
377 }
378
379 cJSON_Delete(config_json.root);
380
381 return true;
382}
383
384
385/*
386 *
387 * Device functions.
388 *
389 */
390
391static xrt_result_t
392rs_ddev_get_tracked_pose(struct xrt_device *xdev,
393 enum xrt_input_name name,
394 int64_t at_timestamp_ns,
395 struct xrt_space_relation *out_relation)
396{
397 struct rs_ddev *rs = rs_ddev(xdev);
398
399 if (name != XRT_INPUT_GENERIC_TRACKER_POSE) {
400 U_LOG_XDEV_UNSUPPORTED_INPUT(&rs->base, u_log_get_global_level(), name);
401 return XRT_ERROR_INPUT_UNSUPPORTED;
402 }
403
404 m_relation_history_get(rs->relation_hist, at_timestamp_ns, out_relation);
405 return XRT_SUCCESS;
406}
407
408static void
409rs_ddev_destroy(struct xrt_device *xdev)
410{
411 struct rs_ddev *rs = rs_ddev(xdev);
412
413 // Destroy the thread object.
414 os_thread_helper_destroy(&rs->oth);
415
416 close_ddev(rs);
417
418 m_relation_history_destroy(&rs->relation_hist);
419
420 free(rs);
421}
422
423
424/*
425 *
426 * 'Exported' functions.
427 *
428 */
429
430struct xrt_device *
431rs_ddev_create(int device_idx)
432{
433 struct rs_ddev *rs = U_DEVICE_ALLOCATE(struct rs_ddev, U_DEVICE_ALLOC_TRACKING_NONE, 1, 0);
434
435 m_relation_history_create(&rs->relation_hist);
436
437 rs->enable_mapping = true;
438 rs->enable_pose_jumping = true;
439 rs->enable_relocalization = true;
440 rs->enable_pose_prediction = true;
441 rs->enable_pose_filtering = true;
442
443 if (load_config(rs)) {
444 U_LOG_D("Used config file");
445 } else {
446 U_LOG_D("Did not use config file");
447 }
448
449 U_LOG_D("Realsense opts are %i %i %i %i %i\n", rs->enable_mapping, rs->enable_pose_jumping,
450 rs->enable_relocalization, rs->enable_pose_prediction, rs->enable_pose_filtering);
451 u_device_populate_function_pointers(&rs->base, rs_ddev_get_tracked_pose, rs_ddev_destroy);
452 rs->base.name = XRT_DEVICE_REALSENSE;
453 rs->base.tracking_origin->type = XRT_TRACKING_TYPE_EXTERNAL_SLAM;
454
455 // Print name.
456 snprintf(rs->base.str, XRT_DEVICE_NAME_LEN, "Intel RealSense Device-SLAM");
457 snprintf(rs->base.serial, XRT_DEVICE_NAME_LEN, "Intel RealSense Device-SLAM");
458
459 rs->base.inputs[0].name = XRT_INPUT_GENERIC_TRACKER_POSE;
460
461 int ret = 0;
462
463
464
465 // Thread and other state.
466 ret = os_thread_helper_init(&rs->oth);
467 if (ret != 0) {
468 U_LOG_E("Failed to init threading!");
469 rs_ddev_destroy(&rs->base);
470 return NULL;
471 }
472
473 ret = create_ddev(rs, device_idx);
474 if (ret != 0) {
475 rs_ddev_destroy(&rs->base);
476 return NULL;
477 }
478
479 ret = os_thread_helper_start(&rs->oth, rs_run_thread, rs);
480 if (ret != 0) {
481 U_LOG_E("Failed to start thread!");
482 rs_ddev_destroy(&rs->base);
483 return NULL;
484 }
485
486 rs->base.supported.orientation_tracking = true;
487 rs->base.supported.position_tracking = true;
488 rs->base.device_type = XRT_DEVICE_TYPE_GENERIC_TRACKER;
489
490 return &rs->base;
491}