d/dai: Update DepthAI driver · matrixfurry.com/monado@6fdd790

+1 -4

src/xrt/drivers/CMakeLists.txt

··· 31 31 endif() 32 32 33 33 if(XRT_BUILD_DRIVER_DEPTHAI) 34 - add_library( 35 - drv_depthai STATIC depthai/depthai_driver.cpp depthai/depthai_tracked_device.c 36 - depthai/depthai_interface.h 37 - ) 34 + add_library(drv_depthai STATIC depthai/depthai_driver.cpp depthai/depthai_interface.h) 38 35 target_link_libraries( 39 36 drv_depthai 40 37 PRIVATE

+46 -29

src/xrt/drivers/depthai/depthai_driver.cpp

··· 11 11 #include "os/os_time.h" 12 12 #include "os/os_threading.h" 13 13 14 + #include "util/u_sink.h" 14 15 #include "xrt/xrt_tracking.h" 15 16 16 17 #include "util/u_var.h" ··· 50 51 51 52 DEBUG_GET_ONCE_LOG_OPTION(depthai_log, "DEPTHAI_LOG", U_LOGGING_INFO) 52 53 DEBUG_GET_ONCE_BOOL_OPTION(depthai_want_floodlight, "DEPTHAI_WANT_FLOODLIGHT", true) 54 + DEBUG_GET_ONCE_NUM_OPTION(depthai_startup_wait_frames, "DEPTHAI_STARTUP_WAIT_FRAMES", 0) 53 55 54 56 55 57 ··· 128 130 xrt_frame_sink *sink[4]; 129 131 xrt_imu_sink *imu_sink; 130 132 133 + struct u_sink_debug debug_sinks[4]; 134 + 131 135 dai::Device *device; 132 136 dai::DataOutputQueue *image_queue; 133 137 dai::DataOutputQueue *imu_queue; ··· 152 156 153 157 bool want_cameras; 154 158 bool want_imu; 159 + bool half_size_ov9282; 160 + 161 + uint32_t first_frames_idx; 162 + uint32_t first_frames_camera_to_watch; 155 163 }; 156 164 157 165 ··· 367 375 368 376 if (depthai->sink[num] == nullptr) { 369 377 DEPTHAI_ERROR(depthai, "No sink waiting for frame! (%u)", num); 378 + return; 379 + } 380 + 381 + if (depthai->first_frames_idx < debug_get_num_option_depthai_startup_wait_frames()) { 382 + if (depthai->first_frames_idx == 0) { 383 + depthai->first_frames_camera_to_watch = num; 384 + } 385 + if (num != depthai->first_frames_camera_to_watch) { 386 + return; 387 + } 388 + depthai->first_frames_idx++; 370 389 return; 371 390 } 372 391 ··· 386 405 387 406 // Push the frame to the sink. 388 407 xrt_sink_push_frame(depthai->sink[num], xf); 408 + u_sink_debug_push_frame(&depthai->debug_sinks[num], xf); 389 409 390 410 // If downstream wants to keep the frame they would have referenced it. 391 411 xrt_frame_reference(&xf, NULL); ··· 429 449 { 430 450 std::shared_ptr<dai::IMUData> imuData = depthai->imu_queue->get<dai::IMUData>(); 431 451 452 + if (depthai->first_frames_idx < debug_get_num_option_depthai_startup_wait_frames()) { 453 + return; 454 + } 455 + 456 + 432 457 std::vector<dai::IMUPacket> imuPackets = imuData->packets; 433 458 434 459 if (imuPackets.size() != 2) { 435 - DEPTHAI_WARN(depthai, "Wrong number of IMU reports!"); 460 + DEPTHAI_ERROR(depthai, "Wrong number of IMU reports!"); 436 461 // Yeah we're not dealing with this. Shouldn't ever happen 437 462 return; 438 463 } ··· 642 667 // OV_9282 L/R 643 668 depthai->width = 1280; 644 669 depthai->height = 800; 670 + if (depthai->half_size_ov9282) { 671 + depthai->width /= 2; 672 + depthai->height /= 2; 673 + depthai->grayscale_sensor_resolution = dai::MonoCameraProperties::SensorResolution::THE_400_P; 674 + } else { 675 + depthai->grayscale_sensor_resolution = dai::MonoCameraProperties::SensorResolution::THE_800_P; 676 + } 645 677 depthai->format = XRT_FORMAT_L8; 646 678 depthai->camera_board_socket = dai::CameraBoardSocket::LEFT; 647 - depthai->grayscale_sensor_resolution = dai::MonoCameraProperties::SensorResolution::THE_800_P; 648 679 depthai->image_orientation = dai::CameraImageOrientation::AUTO; 649 - depthai->fps = 60; // Currently supports up to 60. 650 680 } else { 651 681 // OV_7251 L/R 652 682 depthai->width = 640; ··· 655 685 depthai->camera_board_socket = dai::CameraBoardSocket::LEFT; 656 686 depthai->grayscale_sensor_resolution = dai::MonoCameraProperties::SensorResolution::THE_480_P; 657 687 depthai->image_orientation = dai::CameraImageOrientation::AUTO; 658 - depthai->fps = 60; // Currently supports up to 60. 659 688 } 660 689 661 690 dai::Pipeline p = {}; ··· 952 981 depthai->want_floodlight = debug_get_bool_option_depthai_want_floodlight(); 953 982 depthai->device = d; 954 983 984 + u_var_add_root(depthai, "DepthAI Source", 0); 985 + u_var_add_sink_debug(depthai, &depthai->debug_sinks[0], "RGB"); 986 + u_var_add_sink_debug(depthai, &depthai->debug_sinks[1], "Left"); 987 + u_var_add_sink_debug(depthai, &depthai->debug_sinks[2], "Right"); 988 + u_var_add_sink_debug(depthai, &depthai->debug_sinks[3], "CamD"); 989 + 955 990 // Some debug printing. 956 991 depthai_guess_camera_type(depthai); 957 992 depthai_guess_ir_drivers(depthai); ··· 995 1030 } 996 1031 997 1032 extern "C" struct xrt_fs * 998 - depthai_fs_stereo_grayscale(struct xrt_frame_context *xfctx) 1033 + depthai_fs_slam(struct xrt_frame_context *xfctx, struct depthai_slam_startup_settings *settings) 999 1034 { 1000 1035 struct depthai_fs *depthai = depthai_create_and_do_minimal_setup(); 1001 - depthai->want_cameras = true; 1002 - depthai->want_imu = false; 1003 1036 if (depthai == nullptr) { 1004 1037 return nullptr; 1005 1038 } 1006 1039 1040 + depthai->fps = settings->frames_per_second; 1041 + depthai->want_cameras = settings->want_cameras; 1042 + depthai->want_imu = settings->want_imu; 1043 + depthai->half_size_ov9282 = settings->half_size_ov9282; 1044 + 1045 + 1007 1046 // Last bit is to setup the pipeline. 1008 1047 depthai_setup_stereo_grayscale_pipeline(depthai); 1009 1048 ··· 1057 1096 1058 1097 return &depthai->base; 1059 1098 } 1060 - 1061 - 1062 - // struct xrt_fs * 1063 - // depthai_fs_just_imu(struct xrt_frame_context *xfctx) 1064 - // { 1065 - // { 1066 - // struct depthai_fs *depthai = depthai_create_and_do_minimal_setup(); 1067 - // if (depthai == nullptr) { 1068 - // return nullptr; 1069 - // } 1070 - 1071 - // // Last bit is to setup the pipeline. 1072 - // depthai_setup_stereo_grayscale_pipeline(depthai); 1073 - 1074 - // // And finally add us to the context when we are done. 1075 - // xrt_frame_context_add(xfctx, &depthai->node); 1076 - 1077 - // DEPTHAI_DEBUG(depthai, "DepthAI: Created"); 1078 - 1079 - // return &depthai->base; 1080 - // } 1081 - // } 1082 1099 1083 1100 #ifdef DEPTHAI_HAS_MULTICAM_SUPPORT 1084 1101 extern "C" struct xrt_fs *

+9 -22

src/xrt/drivers/depthai/depthai_interface.h

··· 25 25 26 26 27 27 #define DEPTHAI_VID 0x03e7 28 - // 2485 29 28 #define DEPTHAI_PID 0x2485 30 - // #define DEPTHAI_PID 0xf63b 31 - // #define DEPTHAI_PID_ENUMERATED 0x2485 29 + // FWIW, when the device is actively running, it reboots with the PID 0xf63b. 32 30 31 + struct depthai_slam_startup_settings 32 + { 33 + bool want_cameras; 34 + bool want_imu; 35 + bool half_size_ov9282; 36 + int frames_per_second; 37 + }; 33 38 34 39 int 35 40 depthai_3dof_device_found(struct xrt_prober *xp, ··· 61 66 * @ingroup drv_depthai 62 67 */ 63 68 struct xrt_fs * 64 - depthai_fs_stereo_grayscale(struct xrt_frame_context *xfctx); 65 - 66 - /*! 67 - * Create a DepthAI frameserver using two gray cameras and the IMU. 68 - * Only OAK-D - OAK-D Lite doesn't have an IMU. Custom FFC setups may or may not work. 69 - * 70 - * @ingroup drv_depthai 71 - */ 72 - struct xrt_fs * 73 - depthai_fs_stereo_grayscale_and_imu(struct xrt_frame_context *xfctx); 74 - 75 - /*! 76 - * Create a DepthAI frameserver using two gray cameras. 77 - * Any DepthAI device with an IMU. 78 - * 79 - * @ingroup drv_depthai 80 - */ 81 - struct xrt_fs * 82 - depthai_fs_just_imu(struct xrt_frame_context *xfctx); 69 + depthai_fs_slam(struct xrt_frame_context *xfctx, struct depthai_slam_startup_settings *settings); 83 70 84 71 #ifdef DEPTHAI_HAS_MULTICAM_SUPPORT 85 72 /*!

-254

src/xrt/drivers/depthai/depthai_tracked_device.c

··· 1 - // Copyright 2022, Collabora, Ltd. 2 - // SPDX-License-Identifier: BSL-1.0 3 - /*! 4 - * @file 5 - * @brief Tiny xrt_device to track your head using a DepthAI device. 6 - * @author Moses Turner <moses@collabora.com> 7 - * @ingroup drv_depthai 8 - */ 9 - 10 - #include "os/os_time.h" 11 - #include "os/os_threading.h" 12 - 13 - #include "util/u_sink.h" 14 - #include "xrt/xrt_frame.h" 15 - #include "xrt/xrt_frameserver.h" 16 - #include "xrt/xrt_tracking.h" 17 - 18 - #include "util/u_var.h" 19 - #include "util/u_misc.h" 20 - #include "util/u_debug.h" 21 - #include "util/u_frame.h" 22 - #include "util/u_format.h" 23 - #include "util/u_logging.h" 24 - #include "util/u_trace_marker.h" 25 - #include "math/m_api.h" 26 - 27 - #include "tracking/t_tracking.h" 28 - 29 - #include "depthai_interface.h" 30 - 31 - #include <stdio.h> 32 - #include <assert.h> 33 - #include <unistd.h> 34 - #include <pthread.h> 35 - 36 - #include "depthai_interface.h" 37 - #include "util/u_device.h" 38 - #include "math/m_imu_3dof.h" 39 - #include "math/m_relation_history.h" 40 - 41 - 42 - /* 43 - * 44 - * Printing functions. 45 - * 46 - */ 47 - 48 - #define DEPTHAI_TRACE(d, ...) U_LOG_IFL_T(d->log_level, __VA_ARGS__) 49 - #define DEPTHAI_DEBUG(d, ...) U_LOG_IFL_D(d->log_level, __VA_ARGS__) 50 - #define DEPTHAI_INFO(d, ...) U_LOG_IFL_I(d->log_level, __VA_ARGS__) 51 - #define DEPTHAI_WARN(d, ...) U_LOG_IFL_W(d->log_level, __VA_ARGS__) 52 - #define DEPTHAI_ERROR(d, ...) U_LOG_IFL_E(d->log_level, __VA_ARGS__) 53 - 54 - DEBUG_GET_ONCE_LOG_OPTION(depthai_log, "DEPTHAI_LOG", U_LOGGING_INFO) 55 - 56 - 57 - DEBUG_GET_ONCE_BOOL_OPTION(depthai_3dof, "DEPTHAI_3DOF", false) 58 - DEBUG_GET_ONCE_BOOL_OPTION(depthai_3dof_camera_images, "DEPTHAI_3DOF_CAMERA_IMAGES", false) 59 - 60 - 61 - 62 - struct depthai_xdev 63 - { 64 - struct xrt_device base; 65 - struct m_imu_3dof fusion; 66 - struct xrt_frame_context xfctx; 67 - struct xrt_frame_sink pretty; 68 - struct xrt_imu_sink imu_sink; 69 - struct m_relation_history *rh; 70 - struct u_sink_debug debug_sink; 71 - enum u_logging_level log_level; 72 - }; 73 - 74 - static inline struct depthai_xdev * 75 - depthai_xdev(struct xrt_device *xdev) 76 - { 77 - return (struct depthai_xdev *)xdev; 78 - } 79 - 80 - static void 81 - depthai_3dof_update_inputs(struct xrt_device *xdev) 82 - { 83 - // Empty 84 - } 85 - 86 - static void 87 - depthai_3dof_get_tracked_pose(struct xrt_device *xdev, 88 - enum xrt_input_name name, 89 - uint64_t at_timestamp_ns, 90 - struct xrt_space_relation *out_relation) 91 - { 92 - struct depthai_xdev *dx = depthai_xdev(xdev); 93 - 94 - if (name != XRT_INPUT_GENERIC_TRACKER_POSE) { 95 - DEPTHAI_ERROR(dx, "unknown input name"); 96 - return; 97 - } 98 - 99 - m_relation_history_get(dx->rh, at_timestamp_ns, out_relation); 100 - } 101 - 102 - static void 103 - depthai_3dof_get_view_poses(struct xrt_device *xdev, 104 - const struct xrt_vec3 *default_eye_relation, 105 - uint64_t at_timestamp_ns, 106 - uint32_t view_count, 107 - struct xrt_space_relation *out_head_relation, 108 - struct xrt_fov *out_fovs, 109 - struct xrt_pose *out_poses) 110 - { 111 - assert(false); 112 - } 113 - 114 - static void 115 - depthai_3dof_destroy(struct xrt_device *xdev) 116 - { 117 - struct depthai_xdev *dx = depthai_xdev(xdev); 118 - 119 - xrt_frame_context_destroy_nodes(&dx->xfctx); 120 - m_imu_3dof_close(&dx->fusion); 121 - m_relation_history_destroy(&dx->rh); 122 - u_var_remove_root(dx); 123 - u_device_free(&dx->base); 124 - } 125 - 126 - static void 127 - depthai_pretty_push_frame(struct xrt_frame_sink *sink, struct xrt_frame *frame) 128 - { 129 - struct depthai_xdev *dx = container_of(sink, struct depthai_xdev, pretty); 130 - 131 - u_sink_debug_push_frame(&dx->debug_sink, frame); 132 - } 133 - 134 - static void 135 - depthai_receive_imu_sample(struct xrt_imu_sink *imu_sink, struct xrt_imu_sample *imu_sample) 136 - { 137 - struct depthai_xdev *dx = container_of(imu_sink, struct depthai_xdev, imu_sink); 138 - DEPTHAI_TRACE(dx, "got IMU sample"); 139 - 140 - struct xrt_vec3 a; 141 - struct xrt_vec3 g; 142 - 143 - a.x = imu_sample->accel_m_s2.x; 144 - a.y = imu_sample->accel_m_s2.y; 145 - a.z = imu_sample->accel_m_s2.z; 146 - 147 - g.x = imu_sample->gyro_rad_secs.x; 148 - g.y = imu_sample->gyro_rad_secs.y; 149 - g.z = imu_sample->gyro_rad_secs.z; 150 - 151 - m_imu_3dof_update(&dx->fusion, imu_sample->timestamp_ns, &a, &g); 152 - 153 - struct xrt_space_relation rel = {0}; 154 - rel.relation_flags = (enum xrt_space_relation_flags)(XRT_SPACE_RELATION_ORIENTATION_VALID_BIT | 155 - XRT_SPACE_RELATION_ORIENTATION_TRACKED_BIT); 156 - rel.pose.orientation = dx->fusion.rot; 157 - 158 - // m_quat_rotate(rel.pose,) 159 - struct xrt_quat rot; 160 - // struct xrt_vec3 x = {0,-1,0}; 161 - 162 - 163 - struct xrt_vec3 x = {0, -1, 0}; 164 - struct xrt_vec3 z = {0, 0, -1}; 165 - 166 - math_quat_from_plus_x_z(&x, &z, &rot); 167 - math_quat_rotate(&dx->fusion.rot, &rot, &rel.pose.orientation); 168 - 169 - 170 - 171 - m_relation_history_push(dx->rh, &rel, imu_sample->timestamp_ns); 172 - } 173 - 174 - int 175 - depthai_3dof_device_found(struct xrt_prober *xp, 176 - struct xrt_prober_device **devices, 177 - size_t device_count, 178 - size_t index, 179 - cJSON *attached_data, 180 - struct xrt_device **out_xdev) 181 - { 182 - bool should_do = debug_get_bool_option_depthai_3dof(); 183 - 184 - if (!should_do) { 185 - return 0; 186 - } 187 - 188 - bool camera_images = debug_get_bool_option_depthai_3dof_camera_images(); 189 - 190 - struct xrt_frame_context xfctx; 191 - struct xrt_fs *the_fs = NULL; 192 - if (camera_images) { 193 - the_fs = depthai_fs_stereo_grayscale_and_imu(&xfctx); 194 - } else { 195 - the_fs = depthai_fs_just_imu(&xfctx); 196 - } 197 - if (the_fs == NULL) { 198 - return 0; 199 - } 200 - 201 - struct depthai_xdev *dx = U_DEVICE_ALLOCATE(struct depthai_xdev, U_DEVICE_ALLOC_TRACKING_NONE, 1, 0); 202 - dx->log_level = debug_get_log_option_depthai_log(); 203 - 204 - 205 - dx->xfctx = xfctx; 206 - 207 - m_relation_history_create(&dx->rh); 208 - 209 - 210 - dx->base.update_inputs = depthai_3dof_update_inputs; 211 - dx->base.get_tracked_pose = depthai_3dof_get_tracked_pose; 212 - dx->base.get_view_poses = depthai_3dof_get_view_poses; 213 - dx->base.destroy = depthai_3dof_destroy; 214 - dx->base.name = XRT_DEVICE_DEPTHAI; 215 - dx->base.tracking_origin->type = XRT_TRACKING_TYPE_OTHER; 216 - dx->base.tracking_origin->offset = (struct xrt_pose)XRT_POSE_IDENTITY; 217 - dx->base.inputs[0].name = XRT_INPUT_GENERIC_TRACKER_POSE; 218 - dx->base.orientation_tracking_supported = true; 219 - dx->base.position_tracking_supported = true; 220 - dx->base.device_type = XRT_DEVICE_TYPE_GENERIC_TRACKER; 221 - 222 - 223 - 224 - // Print name. 225 - snprintf(dx->base.str, XRT_DEVICE_NAME_LEN, "DepthAI Head Tracker"); 226 - snprintf(dx->base.serial, XRT_DEVICE_NAME_LEN, "DepthAI Head Tracker"); 227 - 228 - 229 - 230 - struct xrt_slam_sinks tmp = {0}; 231 - 232 - u_var_add_root(dx, "DepthAI Head Tracker", 0); 233 - 234 - if (camera_images) { 235 - dx->pretty.push_frame = depthai_pretty_push_frame; 236 - u_var_add_sink_debug(dx, &dx->debug_sink, "Camera view!"); 237 - u_sink_combiner_create(&dx->xfctx, &dx->pretty, &tmp.left, &tmp.right); 238 - } 239 - 240 - m_imu_3dof_init(&dx->fusion, M_IMU_3DOF_USE_GRAVITY_DUR_300MS); 241 - m_imu_3dof_add_vars(&dx->fusion, dx, ""); 242 - 243 - 244 - 245 - dx->imu_sink.push_imu = depthai_receive_imu_sample; 246 - tmp.imu = &dx->imu_sink; 247 - 248 - 249 - xrt_fs_slam_stream_start(the_fs, &tmp); 250 - 251 - 252 - out_xdev[0] = &dx->base; 253 - return 1; 254 - }

+8 -1

src/xrt/state_trackers/gui/gui_scene_record.c

··· 207 207 return; 208 208 } 209 209 210 - cw->camera.xfs = depthai_fs_stereo_grayscale(&cw->camera.xfctx); 210 + struct depthai_slam_startup_settings settings = {0}; 211 + settings.frames_per_second = 60; 212 + settings.half_size_ov9282 = false; 213 + settings.want_cameras = true; 214 + settings.want_imu = false; 215 + 216 + 217 + cw->camera.xfs = depthai_fs_slam(&cw->camera.xfctx, &settings); 211 218 if (cw->camera.xfs == NULL) { 212 219 U_LOG_W("Could not create depthai camera!"); 213 220 return;

+9 -1

src/xrt/state_trackers/gui/gui_scene_video.c

··· 73 73 { 74 74 vs->xfctx = U_TYPED_CALLOC(struct xrt_frame_context); 75 75 76 - vs->xfs = depthai_fs_stereo_grayscale(vs->xfctx); 76 + 77 + struct depthai_slam_startup_settings settings = {0}; 78 + settings.frames_per_second = 60; 79 + settings.half_size_ov9282 = false; 80 + settings.want_cameras = true; 81 + settings.want_imu = false; 82 + 83 + 84 + vs->xfs = depthai_fs_slam(vs->xfctx, &settings); 77 85 if (vs->xfs == NULL) { 78 86 U_LOG_E("Failed to open DepthAI camera!"); 79 87 free(vs->xfctx);

-4

src/xrt/targets/common/target_lists.c

··· 146 146 {HDK_VID, HDK_PID, hdk_found, "OSVR HDK", "osvr"}, 147 147 #endif // XRT_BUILD_DRIVER_HDK 148 148 149 - #ifdef XRT_BUILD_DRIVER_DEPTHAI 150 - {DEPTHAI_VID, DEPTHAI_PID, depthai_3dof_device_found, "DepthAI Device as Head Tracker", "depthai"}, 151 - #endif 152 - 153 149 #ifdef XRT_BUILD_DRIVER_WMR 154 150 {MICROSOFT_VID, HOLOLENS_SENSORS_PID, wmr_found, "Microsoft HoloLens Sensors", "wmr"}, 155 151 {MICROSOFT_VID, WMR_CONTROLLER_PID, wmr_bt_controller_found, "WMR Bluetooth controller", "wmr"},

Configure Feed

Configure Feed