The open source OpenXR runtime
d/rift_s: Add initial multicamera support
Tested-by: Nova <technobaboo@gmail.com>
authored by
Mateo de Mayo
and committed by
Jakob Bornecrantz
59c3c911
813cb31c
+55
-64
+11
-7
src/xrt/drivers/rift_s/rift_s_camera.c
+11
-7
src/xrt/drivers/rift_s/rift_s_camera.c
···
398
398
u_sink_debug_push_frame(&cam->debug_sinks[0], xf_crop);
399
399
xrt_frame_reference(&xf_crop, NULL);
400
400
401
-
/* Extract left and right frames and push to the tracker */
402
-
struct xrt_frame *left = rift_s_camera_extract_frame(cam, RIFT_S_CAMERA_FRONT_LEFT, xf, &row_data);
403
-
struct xrt_frame *right = rift_s_camera_extract_frame(cam, RIFT_S_CAMERA_FRONT_RIGHT, xf, &row_data);
401
+
/* Extract camera frames and push to the tracker */
402
+
struct xrt_frame *frames[RIFT_S_CAMERA_COUNT] = {0};
403
+
for (int i = 0; i < RIFT_S_CAMERA_COUNT; i++) {
404
+
frames[i] = rift_s_camera_extract_frame(cam, CAM_IDX_TO_ID[i], xf, &row_data);
405
+
}
404
406
405
407
/* Update the exposure for all cameras based on the auto exposure for the left camera view */
406
-
update_expgain(cam, left);
408
+
//! @todo Update expgain independently for each camera like in WMR
409
+
update_expgain(cam, frames[0]);
407
410
408
411
uint64_t frame_ts_ns = (uint64_t)__le64_to_cpu(row_data.data.frame_ts) * OS_NS_PER_USEC;
409
-
rift_s_tracker_push_slam_frames(cam->tracker, frame_ts_ns, left, right);
412
+
rift_s_tracker_push_slam_frames(cam->tracker, frame_ts_ns, frames);
410
413
411
-
xrt_frame_reference(&left, NULL);
412
-
xrt_frame_reference(&right, NULL);
414
+
for (int i = 0; i < RIFT_S_CAMERA_COUNT; i++) {
415
+
xrt_frame_reference(&frames[i], NULL);
416
+
}
413
417
} else {
414
418
struct xrt_rect roi = {.offset = {0, 40}, .extent = {.w = xf->width, .h = 480}};
415
419
struct xrt_frame *xf_crop = NULL;
+1
-1
src/xrt/drivers/rift_s/rift_s_firmware.c
+1
-1
src/xrt/drivers/rift_s/rift_s_firmware.c
+9
-3
src/xrt/drivers/rift_s/rift_s_firmware.h
+9
-3
src/xrt/drivers/rift_s/rift_s_firmware.h
···
37
37
RIFT_S_CAMERA_FRONT_RIGHT = 0x2,
38
38
RIFT_S_CAMERA_FRONT_LEFT = 0x3,
39
39
RIFT_S_CAMERA_SIDE_RIGHT = 0x4,
40
+
RIFT_S_CAMERA_COUNT,
41
+
};
42
+
43
+
//! Order/index of cameras when dealing with multi-camera tracking
44
+
static const enum rift_s_camera_id CAM_IDX_TO_ID[RIFT_S_CAMERA_COUNT] = {
45
+
RIFT_S_CAMERA_FRONT_LEFT, RIFT_S_CAMERA_FRONT_RIGHT, //
46
+
RIFT_S_CAMERA_SIDE_LEFT, RIFT_S_CAMERA_SIDE_RIGHT, //
47
+
RIFT_S_CAMERA_TOP,
40
48
};
41
49
42
50
struct rift_s_imu_calibration
···
78
86
struct rift_s_fisheye62_distortion distortion;
79
87
};
80
88
81
-
#define RIFT_S_MAX_CAMERAS 5
82
-
83
89
struct rift_s_camera_calibration_block
84
90
{
85
-
struct rift_s_camera_calibration cameras[RIFT_S_MAX_CAMERAS];
91
+
struct rift_s_camera_calibration cameras[RIFT_S_CAMERA_COUNT];
86
92
};
87
93
88
94
/* Rift S controller LED entry */
+33
-51
src/xrt/drivers/rift_s/rift_s_tracker.c
+33
-51
src/xrt/drivers/rift_s/rift_s_tracker.c
···
156
156
const int CAMERA_FREQUENCY = 30;
157
157
158
158
struct rift_s_camera_calibration_block *camera_calibration = &hmd_config->camera_calibration;
159
-
struct rift_s_camera_calibration *left = &camera_calibration->cameras[RIFT_S_CAMERA_FRONT_LEFT];
160
-
struct rift_s_camera_calibration *right = &camera_calibration->cameras[RIFT_S_CAMERA_FRONT_RIGHT];
161
159
162
160
/* Compute the IMU from cam transform for each cam */
163
161
struct xrt_pose device_from_imu, imu_from_device;
164
-
165
162
math_pose_from_isometry(&hmd_config->imu_calibration.device_from_imu, &device_from_imu);
166
163
math_pose_invert(&device_from_imu, &imu_from_device);
167
164
168
-
struct xrt_pose device_from_left, device_from_right;
169
-
math_pose_from_isometry(&left->device_from_camera, &device_from_left);
170
-
math_pose_from_isometry(&right->device_from_camera, &device_from_right);
165
+
t->slam_calib.cam_count = RIFT_S_CAMERA_COUNT;
166
+
for (int i = 0; i < RIFT_S_CAMERA_COUNT; i++) {
167
+
enum rift_s_camera_id cam_id = CAM_IDX_TO_ID[i];
168
+
struct rift_s_camera_calibration *cam = &camera_calibration->cameras[cam_id];
171
169
172
-
struct xrt_pose P_imu_left_cam, P_imu_right_cam;
173
-
math_pose_transform(&imu_from_device, &device_from_left, &P_imu_left_cam);
174
-
math_pose_transform(&imu_from_device, &device_from_right, &P_imu_right_cam);
175
-
176
-
struct xrt_matrix_4x4 T_imu_left_cam, T_imu_right_cam;
177
-
math_matrix_4x4_isometry_from_pose(&P_imu_left_cam, &T_imu_left_cam);
178
-
math_matrix_4x4_isometry_from_pose(&P_imu_right_cam, &T_imu_right_cam);
179
-
180
-
RIFT_S_DEBUG("IMU left cam pose %f %f %f orient %f %f %f %f", P_imu_left_cam.position.x,
181
-
P_imu_left_cam.position.y, P_imu_left_cam.position.z, P_imu_left_cam.orientation.x,
182
-
P_imu_left_cam.orientation.y, P_imu_left_cam.orientation.z, P_imu_left_cam.orientation.w);
170
+
struct xrt_pose device_from_cam;
171
+
math_pose_from_isometry(&cam->device_from_camera, &device_from_cam);
183
172
184
-
RIFT_S_DEBUG("IMU right cam pose %f %f %f orient %f %f %f %f", P_imu_right_cam.position.x,
185
-
P_imu_right_cam.position.y, P_imu_right_cam.position.z, P_imu_right_cam.orientation.x,
186
-
P_imu_right_cam.orientation.y, P_imu_right_cam.orientation.z, P_imu_right_cam.orientation.w);
173
+
struct xrt_pose P_imu_cam;
174
+
math_pose_transform(&imu_from_device, &device_from_cam, &P_imu_cam);
187
175
188
-
struct t_slam_camera_calibration calib0 = {
189
-
.base = rift_s_get_cam_calib(&hmd_config->camera_calibration, RIFT_S_CAMERA_FRONT_LEFT),
190
-
.frequency = CAMERA_FREQUENCY,
191
-
.T_imu_cam = T_imu_left_cam,
192
-
};
176
+
struct xrt_matrix_4x4 T_imu_cam;
177
+
math_matrix_4x4_isometry_from_pose(&P_imu_cam, &T_imu_cam);
193
178
194
-
struct t_slam_camera_calibration calib1 = {
195
-
.base = rift_s_get_cam_calib(&hmd_config->camera_calibration, RIFT_S_CAMERA_FRONT_RIGHT),
196
-
.frequency = CAMERA_FREQUENCY,
197
-
.T_imu_cam = T_imu_right_cam,
198
-
};
179
+
RIFT_S_DEBUG("IMU cam%d cam pose %f %f %f orient %f %f %f %f", i, P_imu_cam.position.x,
180
+
P_imu_cam.position.y, P_imu_cam.position.z, P_imu_cam.orientation.x,
181
+
P_imu_cam.orientation.y, P_imu_cam.orientation.z, P_imu_cam.orientation.w);
199
182
200
-
t->slam_calib.cam_count = 2;
201
-
t->slam_calib.cams[0] = calib0;
202
-
t->slam_calib.cams[1] = calib1;
183
+
struct t_slam_camera_calibration calib = {
184
+
.base = rift_s_get_cam_calib(&hmd_config->camera_calibration, cam_id),
185
+
.frequency = CAMERA_FREQUENCY,
186
+
.T_imu_cam = T_imu_cam,
187
+
};
188
+
t->slam_calib.cams[i] = calib;
189
+
}
203
190
}
204
191
205
192
static void
···
221
208
t_slam_fill_default_config(&config);
222
209
223
210
/* No need to refcount these parameters */
224
-
config.cam_count = 2;
211
+
config.cam_count = RIFT_S_CAMERA_COUNT;
225
212
config.slam_calib = &t->slam_calib;
226
213
227
214
int create_status = t_slam_create(xfctx, &config, &t->tracking.slam, &sinks);
···
424
411
// Setup sinks depending on tracking configuration
425
412
struct xrt_slam_sinks entry_sinks = {0};
426
413
if (slam_enabled && hand_enabled) {
427
-
struct xrt_frame_sink *entry_left_sink = NULL;
428
-
struct xrt_frame_sink *entry_right_sink = NULL;
414
+
struct xrt_frame_sink *entry_cam0_sink = NULL;
415
+
struct xrt_frame_sink *entry_cam1_sink = NULL;
429
416
430
-
u_sink_split_create(xfctx, slam_sinks->cams[0], hand_sinks->cams[0], &entry_left_sink);
431
-
u_sink_split_create(xfctx, slam_sinks->cams[1], hand_sinks->cams[1], &entry_right_sink);
417
+
u_sink_split_create(xfctx, slam_sinks->cams[0], hand_sinks->cams[0], &entry_cam0_sink);
418
+
u_sink_split_create(xfctx, slam_sinks->cams[1], hand_sinks->cams[1], &entry_cam1_sink);
432
419
433
420
entry_sinks = *slam_sinks;
434
-
entry_sinks.cam_count = 2;
435
-
entry_sinks.cams[0] = entry_left_sink;
436
-
entry_sinks.cams[1] = entry_right_sink;
421
+
entry_sinks.cams[0] = entry_cam0_sink;
422
+
entry_sinks.cams[1] = entry_cam1_sink;
437
423
} else if (slam_enabled) {
438
424
entry_sinks = *slam_sinks;
439
425
} else if (hand_enabled) {
···
555
541
void
556
542
rift_s_tracker_push_slam_frames(struct rift_s_tracker *t,
557
543
uint64_t frame_ts_ns,
558
-
struct xrt_frame *left_frame,
559
-
struct xrt_frame *right_frame)
544
+
struct xrt_frame *frames[RIFT_S_CAMERA_COUNT])
560
545
{
561
546
timepoint_ns frame_time;
562
547
···
601
586
t->last_frame_time = frame_time;
602
587
os_mutex_unlock(&t->mutex);
603
588
604
-
if (t->slam_sinks.cams[0]) {
605
-
left_frame->timestamp = frame_time;
606
-
xrt_sink_push_frame(t->slam_sinks.cams[0], left_frame);
607
-
}
608
-
609
-
if (t->slam_sinks.cams[1]) {
610
-
right_frame->timestamp = frame_time;
611
-
xrt_sink_push_frame(t->slam_sinks.cams[1], right_frame);
589
+
for (int i = 0; i < RIFT_S_CAMERA_COUNT; i++) {
590
+
if (t->slam_sinks.cams[i]) {
591
+
frames[i]->timestamp = frame_time;
592
+
xrt_sink_push_frame(t->slam_sinks.cams[i], frames[i]);
593
+
}
612
594
}
613
595
}
614
596
+1
-2
src/xrt/drivers/rift_s/rift_s_tracker.h
+1
-2
src/xrt/drivers/rift_s/rift_s_tracker.h
···
148
148
void
149
149
rift_s_tracker_push_slam_frames(struct rift_s_tracker *t,
150
150
uint64_t frame_ts_ns,
151
-
struct xrt_frame *left,
152
-
struct xrt_frame *right);
151
+
struct xrt_frame *frames[RIFT_S_CAMERA_COUNT]);
153
152
void
154
153
rift_s_tracker_get_tracked_pose(struct rift_s_tracker *t,
155
154
enum rift_s_tracker_pose pose,