The open source OpenXR runtime
d/rs: remove superfluous rs_update_offset; add config options
+102
-53
+1
-1
src/xrt/drivers/meson.build
+1
-1
src/xrt/drivers/meson.build
+101
-49
src/xrt/drivers/realsense/rs_6dof.c
+101
-49
src/xrt/drivers/realsense/rs_6dof.c
···
4
4
/*!
5
5
* @file
6
6
* @brief RealSense helper driver for 6DOF tracking.
7
+
* @author Moses Turner <mosesturner@protonmail.com>
7
8
* @author Nova King <technobaboo@gmail.com>
8
9
* @author Jakob Bornecrantz <jakob@collabora.com>
9
10
* @ingroup drv_rs
···
21
22
#include "util/u_time.h"
22
23
#include "util/u_device.h"
23
24
#include "util/u_logging.h"
25
+
26
+
#include "util/u_json.h"
27
+
#include "util/u_config_json.h"
24
28
25
29
#include <librealsense2/rs.h>
26
30
#include <librealsense2/h/rs_pipeline.h>
···
50
54
uint64_t relation_timestamp_ns;
51
55
struct xrt_space_relation relation;
52
56
53
-
// arbitrary offset to apply to the pose the t265 gives us
54
-
struct xrt_pose offset;
57
+
struct os_thread_helper oth;
55
58
56
-
struct os_thread_helper oth;
59
+
bool enable_mapping;
60
+
bool enable_pose_jumping;
61
+
bool enable_relocalization;
62
+
bool enable_pose_prediction;
63
+
bool enable_pose_filtering; // Forward compatibility for when that 1-euro filter is working
57
64
58
65
rs2_context *ctx;
59
66
rs2_pipeline *pipe;
···
115
122
}
116
123
}
117
124
125
+
#define CHECK_RS2() \
126
+
do { \
127
+
if (check_error(rs, e) != 0) { \
128
+
close_6dof(rs); \
129
+
return 1; \
130
+
} \
131
+
} while (0)
132
+
133
+
118
134
/*!
119
135
* Create all RealSense resources needed for 6DOF tracking.
120
136
*/
···
125
141
rs2_error *e = NULL;
126
142
127
143
rs->ctx = rs2_create_context(RS2_API_VERSION, &e);
128
-
if (check_error(rs, e) != 0) {
129
-
close_6dof(rs);
130
-
return 1;
131
-
}
144
+
CHECK_RS2();
132
145
133
146
rs2_device_list *device_list = rs2_query_devices(rs->ctx, &e);
134
-
if (check_error(rs, e) != 0) {
135
-
close_6dof(rs);
136
-
return 1;
137
-
}
147
+
CHECK_RS2();
138
148
139
149
int dev_count = rs2_get_device_count(device_list, &e);
140
-
if (check_error(rs, e) != 0) {
141
-
close_6dof(rs);
142
-
return 1;
143
-
}
150
+
CHECK_RS2();
144
151
145
152
U_LOG_D("There are %d connected RealSense devices.", dev_count);
146
153
if (0 == dev_count) {
···
149
156
}
150
157
151
158
rs->pipe = rs2_create_pipeline(rs->ctx, &e);
152
-
if (check_error(rs, e) != 0) {
153
-
close_6dof(rs);
154
-
return 1;
155
-
}
159
+
CHECK_RS2();
156
160
157
161
rs->config = rs2_create_config(&e);
158
-
if (check_error(rs, e) != 0) {
159
-
close_6dof(rs);
160
-
return 1;
161
-
}
162
+
CHECK_RS2();
162
163
163
164
rs2_config_enable_stream(rs->config, //
164
165
RS2_STREAM_POSE, // Type
···
168
169
RS2_FORMAT_6DOF, // Format
169
170
200, // FPS
170
171
&e);
171
-
if (check_error(rs, e) != 0) {
172
-
close_6dof(rs);
173
-
return 1;
172
+
CHECK_RS2();
173
+
174
+
rs2_pipeline_profile *prof = rs2_config_resolve(rs->config, rs->pipe, &e);
175
+
CHECK_RS2();
176
+
177
+
rs2_device *cameras = rs2_pipeline_profile_get_device(prof, &e);
178
+
CHECK_RS2();
179
+
180
+
rs2_sensor_list *sensors = rs2_query_sensors(cameras, &e);
181
+
CHECK_RS2();
182
+
183
+
rs2_sensor *sensor = rs2_create_sensor(sensors, 0, &e);
184
+
CHECK_RS2();
185
+
186
+
rs2_set_option((rs2_options *)sensor, RS2_OPTION_ENABLE_MAPPING, rs->enable_mapping ? 1.0f : 0.0f, &e);
187
+
CHECK_RS2();
188
+
189
+
if (rs->enable_mapping) {
190
+
// Neither of these options mean anything if mapping is off; in fact it errors out
191
+
// if we mess with these
192
+
// with mapping off
193
+
rs2_set_option((rs2_options *)sensor, RS2_OPTION_ENABLE_RELOCALIZATION,
194
+
rs->enable_relocalization ? 1.0f : 0.0f, &e);
195
+
CHECK_RS2();
196
+
197
+
rs2_set_option((rs2_options *)sensor, RS2_OPTION_ENABLE_POSE_JUMPING,
198
+
rs->enable_pose_jumping ? 1.0f : 0.0f, &e);
199
+
CHECK_RS2();
174
200
}
175
201
176
202
rs->profile = rs2_pipeline_start_with_config(rs->pipe, rs->config, &e);
177
-
if (check_error(rs, e) != 0) {
178
-
close_6dof(rs);
179
-
return 1;
180
-
}
203
+
CHECK_RS2();
181
204
182
205
return 0;
183
206
}
184
-
185
-
186
-
void
187
-
rs_update_offset(struct xrt_pose offset, struct xrt_device *xdev)
188
-
{
189
-
struct rs_6dof *rs = rs_6dof(xdev);
190
-
memcpy(&rs->offset, &offset, sizeof(struct xrt_pose));
191
-
}
192
-
193
207
194
208
/*!
195
209
* Process a frame as 6DOF data, does not assume ownership of the frame.
···
354
368
355
369
int64_t diff_prediction_ns = 0;
356
370
diff_prediction_ns = at_timestamp_ns - relation_timestamp_ns;
357
-
struct xrt_space_relation relation;
358
371
359
372
double delta_s = time_ns_to_s(diff_prediction_ns);
360
-
m_predict_relation(&relation_not_predicted, delta_s, &relation);
361
-
362
-
struct xrt_space_graph xsg = {0};
363
-
m_space_graph_add_pose(&xsg, &rs->offset);
364
-
m_space_graph_add_relation(&xsg, &relation);
365
-
m_space_graph_resolve(&xsg, out_relation);
373
+
if (rs->enable_pose_prediction) {
374
+
m_predict_relation(&relation_not_predicted, delta_s, out_relation);
375
+
} else {
376
+
*out_relation = relation_not_predicted;
377
+
}
366
378
}
367
379
static void
368
380
rs_6dof_get_view_pose(struct xrt_device *xdev,
···
390
402
rs_6dof_create(void)
391
403
{
392
404
struct rs_6dof *rs = U_DEVICE_ALLOCATE(struct rs_6dof, U_DEVICE_ALLOC_TRACKING_NONE, 1, 0);
393
-
rs->offset =
394
-
(struct xrt_pose){.orientation = {.x = 0, .y = 0, .z = 0, .w = 1}, .position = {.x = 0, .y = 0, .z = 0}};
395
-
int ret = 0;
405
+
406
+
rs->enable_mapping = true;
407
+
rs->enable_pose_jumping = true;
408
+
rs->enable_relocalization = true;
409
+
rs->enable_pose_prediction = true;
410
+
rs->enable_pose_filtering = true;
411
+
412
+
struct u_config_json config_json;
413
+
u_config_json_open_or_create_main_file(&config_json);
414
+
cJSON *realsense_config_json = cJSON_GetObjectItemCaseSensitive(config_json.root, "config_realsense");
415
+
if (realsense_config_json != NULL) {
416
+
cJSON *mapping = cJSON_GetObjectItemCaseSensitive(realsense_config_json, "enable_mapping");
417
+
cJSON *pose_jumping = cJSON_GetObjectItemCaseSensitive(realsense_config_json, "enable_pose_jumping");
418
+
cJSON *relocalization =
419
+
cJSON_GetObjectItemCaseSensitive(realsense_config_json, "enable_relocalization");
420
+
cJSON *pose_prediction =
421
+
cJSON_GetObjectItemCaseSensitive(realsense_config_json, "enable_pose_prediction");
422
+
cJSON *pose_filtering =
423
+
cJSON_GetObjectItemCaseSensitive(realsense_config_json, "enable_pose_filtering");
424
+
425
+
// if json key isn't in the json, default to true. if it is in there, use json value
426
+
if (mapping != NULL) {
427
+
rs->enable_mapping = cJSON_IsTrue(mapping);
428
+
}
429
+
if (pose_jumping != NULL) {
430
+
rs->enable_pose_jumping = cJSON_IsTrue(pose_jumping);
431
+
}
432
+
if (relocalization != NULL) {
433
+
rs->enable_relocalization = cJSON_IsTrue(relocalization);
434
+
}
435
+
if (pose_prediction != NULL) {
436
+
rs->enable_pose_prediction = cJSON_IsTrue(pose_prediction);
437
+
}
438
+
if (pose_filtering != NULL) {
439
+
rs->enable_pose_filtering = cJSON_IsTrue(pose_filtering);
440
+
}
441
+
}
442
+
U_LOG_D("Realsense opts are %i %i %i %i %i\n", rs->enable_mapping, rs->enable_pose_jumping,
443
+
rs->enable_relocalization, rs->enable_pose_prediction, rs->enable_pose_filtering);
396
444
397
445
rs->base.update_inputs = rs_6dof_update_inputs;
398
446
rs->base.get_tracked_pose = rs_6dof_get_tracked_pose;
···
408
456
snprintf(rs->base.serial, XRT_DEVICE_NAME_LEN, "Intel RealSense 6-DOF");
409
457
410
458
rs->base.inputs[0].name = XRT_INPUT_GENERIC_TRACKER_POSE;
459
+
460
+
int ret = 0;
461
+
462
+
411
463
412
464
// Thread and other state.
413
465
ret = os_thread_helper_init(&rs->oth);