From 6fdd790da095edb2905639ca909f98a60bd042e5 Mon Sep 17 00:00:00 2001 From: Moses Turner Date: Wed, 21 Sep 2022 06:27:38 -0500 Subject: [PATCH] d/dai: Update DepthAI driver * Removed depthai_tracked_device - now you create a "SLAM" device, plug any frameserver into it and you're done * Consolidated the grayscale frameservers into just one that gives you SLAM sinks * Allows for different framerates and half-size for ov9282s * Added debug frame sinks * Added the ability to wait at startup for a number of frames for the streams to stabilize before submitting them to SLAM --- src/xrt/drivers/CMakeLists.txt | 5 +- src/xrt/drivers/depthai/depthai_driver.cpp | 75 ++++-- src/xrt/drivers/depthai/depthai_interface.h | 31 +-- .../drivers/depthai/depthai_tracked_device.c | 254 ------------------ src/xrt/state_trackers/gui/gui_scene_record.c | 9 +- src/xrt/state_trackers/gui/gui_scene_video.c | 10 +- src/xrt/targets/common/target_lists.c | 4 - 7 files changed, 73 insertions(+), 315 deletions(-) delete mode 100644 src/xrt/drivers/depthai/depthai_tracked_device.c diff --git a/src/xrt/drivers/CMakeLists.txt b/src/xrt/drivers/CMakeLists.txt index 9db5b2fb7..3f2b0377f 100644 --- a/src/xrt/drivers/CMakeLists.txt +++ b/src/xrt/drivers/CMakeLists.txt @@ -31,10 +31,7 @@ if(XRT_BUILD_DRIVER_DAYDREAM) endif() if(XRT_BUILD_DRIVER_DEPTHAI) - add_library( - drv_depthai STATIC depthai/depthai_driver.cpp depthai/depthai_tracked_device.c - depthai/depthai_interface.h - ) + add_library(drv_depthai STATIC depthai/depthai_driver.cpp depthai/depthai_interface.h) target_link_libraries( drv_depthai PRIVATE diff --git a/src/xrt/drivers/depthai/depthai_driver.cpp b/src/xrt/drivers/depthai/depthai_driver.cpp index 3f65ef3e5..2740c3c4d 100644 --- a/src/xrt/drivers/depthai/depthai_driver.cpp +++ b/src/xrt/drivers/depthai/depthai_driver.cpp @@ -11,6 +11,7 @@ #include "os/os_time.h" #include "os/os_threading.h" +#include "util/u_sink.h" #include "xrt/xrt_tracking.h" #include "util/u_var.h" @@ -50,6 +51,7 @@ DEBUG_GET_ONCE_LOG_OPTION(depthai_log, "DEPTHAI_LOG", U_LOGGING_INFO) DEBUG_GET_ONCE_BOOL_OPTION(depthai_want_floodlight, "DEPTHAI_WANT_FLOODLIGHT", true) +DEBUG_GET_ONCE_NUM_OPTION(depthai_startup_wait_frames, "DEPTHAI_STARTUP_WAIT_FRAMES", 0) @@ -128,6 +130,8 @@ struct depthai_fs xrt_frame_sink *sink[4]; xrt_imu_sink *imu_sink; + struct u_sink_debug debug_sinks[4]; + dai::Device *device; dai::DataOutputQueue *image_queue; dai::DataOutputQueue *imu_queue; @@ -152,6 +156,10 @@ struct depthai_fs bool want_cameras; bool want_imu; + bool half_size_ov9282; + + uint32_t first_frames_idx; + uint32_t first_frames_camera_to_watch; }; @@ -370,6 +378,17 @@ depthai_do_one_frame(struct depthai_fs *depthai) return; } + if (depthai->first_frames_idx < debug_get_num_option_depthai_startup_wait_frames()) { + if (depthai->first_frames_idx == 0) { + depthai->first_frames_camera_to_watch = num; + } + if (num != depthai->first_frames_camera_to_watch) { + return; + } + depthai->first_frames_idx++; + return; + } + // Create a wrapper that will keep the frame alive as long as the frame was alive. DepthAIFrameWrapper *dfw = new DepthAIFrameWrapper(imgFrame); @@ -386,6 +405,7 @@ depthai_do_one_frame(struct depthai_fs *depthai) // Push the frame to the sink. xrt_sink_push_frame(depthai->sink[num], xf); + u_sink_debug_push_frame(&depthai->debug_sinks[num], xf); // If downstream wants to keep the frame they would have referenced it. xrt_frame_reference(&xf, NULL); @@ -429,10 +449,15 @@ depthai_do_one_imu_frame(struct depthai_fs *depthai) { std::shared_ptr imuData = depthai->imu_queue->get(); + if (depthai->first_frames_idx < debug_get_num_option_depthai_startup_wait_frames()) { + return; + } + + std::vector imuPackets = imuData->packets; if (imuPackets.size() != 2) { - DEPTHAI_WARN(depthai, "Wrong number of IMU reports!"); + DEPTHAI_ERROR(depthai, "Wrong number of IMU reports!"); // Yeah we're not dealing with this. Shouldn't ever happen return; } @@ -642,11 +667,16 @@ depthai_setup_stereo_grayscale_pipeline(struct depthai_fs *depthai) // OV_9282 L/R depthai->width = 1280; depthai->height = 800; + if (depthai->half_size_ov9282) { + depthai->width /= 2; + depthai->height /= 2; + depthai->grayscale_sensor_resolution = dai::MonoCameraProperties::SensorResolution::THE_400_P; + } else { + depthai->grayscale_sensor_resolution = dai::MonoCameraProperties::SensorResolution::THE_800_P; + } depthai->format = XRT_FORMAT_L8; depthai->camera_board_socket = dai::CameraBoardSocket::LEFT; - depthai->grayscale_sensor_resolution = dai::MonoCameraProperties::SensorResolution::THE_800_P; depthai->image_orientation = dai::CameraImageOrientation::AUTO; - depthai->fps = 60; // Currently supports up to 60. } else { // OV_7251 L/R depthai->width = 640; @@ -655,7 +685,6 @@ depthai_setup_stereo_grayscale_pipeline(struct depthai_fs *depthai) depthai->camera_board_socket = dai::CameraBoardSocket::LEFT; depthai->grayscale_sensor_resolution = dai::MonoCameraProperties::SensorResolution::THE_480_P; depthai->image_orientation = dai::CameraImageOrientation::AUTO; - depthai->fps = 60; // Currently supports up to 60. } dai::Pipeline p = {}; @@ -952,6 +981,12 @@ depthai_create_and_do_minimal_setup(void) depthai->want_floodlight = debug_get_bool_option_depthai_want_floodlight(); depthai->device = d; + u_var_add_root(depthai, "DepthAI Source", 0); + u_var_add_sink_debug(depthai, &depthai->debug_sinks[0], "RGB"); + u_var_add_sink_debug(depthai, &depthai->debug_sinks[1], "Left"); + u_var_add_sink_debug(depthai, &depthai->debug_sinks[2], "Right"); + u_var_add_sink_debug(depthai, &depthai->debug_sinks[3], "CamD"); + // Some debug printing. depthai_guess_camera_type(depthai); depthai_guess_ir_drivers(depthai); @@ -995,15 +1030,19 @@ depthai_fs_monocular_rgb(struct xrt_frame_context *xfctx) } extern "C" struct xrt_fs * -depthai_fs_stereo_grayscale(struct xrt_frame_context *xfctx) +depthai_fs_slam(struct xrt_frame_context *xfctx, struct depthai_slam_startup_settings *settings) { struct depthai_fs *depthai = depthai_create_and_do_minimal_setup(); - depthai->want_cameras = true; - depthai->want_imu = false; if (depthai == nullptr) { return nullptr; } + depthai->fps = settings->frames_per_second; + depthai->want_cameras = settings->want_cameras; + depthai->want_imu = settings->want_imu; + depthai->half_size_ov9282 = settings->half_size_ov9282; + + // Last bit is to setup the pipeline. depthai_setup_stereo_grayscale_pipeline(depthai); @@ -1058,28 +1097,6 @@ depthai_fs_just_imu(struct xrt_frame_context *xfctx) return &depthai->base; } - -// struct xrt_fs * -// depthai_fs_just_imu(struct xrt_frame_context *xfctx) -// { -// { -// struct depthai_fs *depthai = depthai_create_and_do_minimal_setup(); -// if (depthai == nullptr) { -// return nullptr; -// } - -// // Last bit is to setup the pipeline. -// depthai_setup_stereo_grayscale_pipeline(depthai); - -// // And finally add us to the context when we are done. -// xrt_frame_context_add(xfctx, &depthai->node); - -// DEPTHAI_DEBUG(depthai, "DepthAI: Created"); - -// return &depthai->base; -// } -// } - #ifdef DEPTHAI_HAS_MULTICAM_SUPPORT extern "C" struct xrt_fs * depthai_fs_stereo_rgb(struct xrt_frame_context *xfctx) diff --git a/src/xrt/drivers/depthai/depthai_interface.h b/src/xrt/drivers/depthai/depthai_interface.h index eee09073a..abf9a0058 100644 --- a/src/xrt/drivers/depthai/depthai_interface.h +++ b/src/xrt/drivers/depthai/depthai_interface.h @@ -25,11 +25,16 @@ struct t_stereo_camera_calibration; #define DEPTHAI_VID 0x03e7 -// 2485 #define DEPTHAI_PID 0x2485 -// #define DEPTHAI_PID 0xf63b -// #define DEPTHAI_PID_ENUMERATED 0x2485 +// FWIW, when the device is actively running, it reboots with the PID 0xf63b. +struct depthai_slam_startup_settings +{ + bool want_cameras; + bool want_imu; + bool half_size_ov9282; + int frames_per_second; +}; int depthai_3dof_device_found(struct xrt_prober *xp, @@ -61,25 +66,7 @@ depthai_fs_monocular_rgb(struct xrt_frame_context *xfctx); * @ingroup drv_depthai */ struct xrt_fs * -depthai_fs_stereo_grayscale(struct xrt_frame_context *xfctx); - -/*! - * Create a DepthAI frameserver using two gray cameras and the IMU. - * Only OAK-D - OAK-D Lite doesn't have an IMU. Custom FFC setups may or may not work. - * - * @ingroup drv_depthai - */ -struct xrt_fs * -depthai_fs_stereo_grayscale_and_imu(struct xrt_frame_context *xfctx); - -/*! - * Create a DepthAI frameserver using two gray cameras. - * Any DepthAI device with an IMU. - * - * @ingroup drv_depthai - */ -struct xrt_fs * -depthai_fs_just_imu(struct xrt_frame_context *xfctx); +depthai_fs_slam(struct xrt_frame_context *xfctx, struct depthai_slam_startup_settings *settings); #ifdef DEPTHAI_HAS_MULTICAM_SUPPORT /*! diff --git a/src/xrt/drivers/depthai/depthai_tracked_device.c b/src/xrt/drivers/depthai/depthai_tracked_device.c deleted file mode 100644 index 19449bb4d..000000000 --- a/src/xrt/drivers/depthai/depthai_tracked_device.c +++ /dev/null @@ -1,254 +0,0 @@ -// Copyright 2022, Collabora, Ltd. -// SPDX-License-Identifier: BSL-1.0 -/*! - * @file - * @brief Tiny xrt_device to track your head using a DepthAI device. - * @author Moses Turner - * @ingroup drv_depthai - */ - -#include "os/os_time.h" -#include "os/os_threading.h" - -#include "util/u_sink.h" -#include "xrt/xrt_frame.h" -#include "xrt/xrt_frameserver.h" -#include "xrt/xrt_tracking.h" - -#include "util/u_var.h" -#include "util/u_misc.h" -#include "util/u_debug.h" -#include "util/u_frame.h" -#include "util/u_format.h" -#include "util/u_logging.h" -#include "util/u_trace_marker.h" -#include "math/m_api.h" - -#include "tracking/t_tracking.h" - -#include "depthai_interface.h" - -#include -#include -#include -#include - -#include "depthai_interface.h" -#include "util/u_device.h" -#include "math/m_imu_3dof.h" -#include "math/m_relation_history.h" - - -/* - * - * Printing functions. - * - */ - -#define DEPTHAI_TRACE(d, ...) U_LOG_IFL_T(d->log_level, __VA_ARGS__) -#define DEPTHAI_DEBUG(d, ...) U_LOG_IFL_D(d->log_level, __VA_ARGS__) -#define DEPTHAI_INFO(d, ...) U_LOG_IFL_I(d->log_level, __VA_ARGS__) -#define DEPTHAI_WARN(d, ...) U_LOG_IFL_W(d->log_level, __VA_ARGS__) -#define DEPTHAI_ERROR(d, ...) U_LOG_IFL_E(d->log_level, __VA_ARGS__) - -DEBUG_GET_ONCE_LOG_OPTION(depthai_log, "DEPTHAI_LOG", U_LOGGING_INFO) - - -DEBUG_GET_ONCE_BOOL_OPTION(depthai_3dof, "DEPTHAI_3DOF", false) -DEBUG_GET_ONCE_BOOL_OPTION(depthai_3dof_camera_images, "DEPTHAI_3DOF_CAMERA_IMAGES", false) - - - -struct depthai_xdev -{ - struct xrt_device base; - struct m_imu_3dof fusion; - struct xrt_frame_context xfctx; - struct xrt_frame_sink pretty; - struct xrt_imu_sink imu_sink; - struct m_relation_history *rh; - struct u_sink_debug debug_sink; - enum u_logging_level log_level; -}; - -static inline struct depthai_xdev * -depthai_xdev(struct xrt_device *xdev) -{ - return (struct depthai_xdev *)xdev; -} - -static void -depthai_3dof_update_inputs(struct xrt_device *xdev) -{ - // Empty -} - -static void -depthai_3dof_get_tracked_pose(struct xrt_device *xdev, - enum xrt_input_name name, - uint64_t at_timestamp_ns, - struct xrt_space_relation *out_relation) -{ - struct depthai_xdev *dx = depthai_xdev(xdev); - - if (name != XRT_INPUT_GENERIC_TRACKER_POSE) { - DEPTHAI_ERROR(dx, "unknown input name"); - return; - } - - m_relation_history_get(dx->rh, at_timestamp_ns, out_relation); -} - -static void -depthai_3dof_get_view_poses(struct xrt_device *xdev, - const struct xrt_vec3 *default_eye_relation, - uint64_t at_timestamp_ns, - uint32_t view_count, - struct xrt_space_relation *out_head_relation, - struct xrt_fov *out_fovs, - struct xrt_pose *out_poses) -{ - assert(false); -} - -static void -depthai_3dof_destroy(struct xrt_device *xdev) -{ - struct depthai_xdev *dx = depthai_xdev(xdev); - - xrt_frame_context_destroy_nodes(&dx->xfctx); - m_imu_3dof_close(&dx->fusion); - m_relation_history_destroy(&dx->rh); - u_var_remove_root(dx); - u_device_free(&dx->base); -} - -static void -depthai_pretty_push_frame(struct xrt_frame_sink *sink, struct xrt_frame *frame) -{ - struct depthai_xdev *dx = container_of(sink, struct depthai_xdev, pretty); - - u_sink_debug_push_frame(&dx->debug_sink, frame); -} - -static void -depthai_receive_imu_sample(struct xrt_imu_sink *imu_sink, struct xrt_imu_sample *imu_sample) -{ - struct depthai_xdev *dx = container_of(imu_sink, struct depthai_xdev, imu_sink); - DEPTHAI_TRACE(dx, "got IMU sample"); - - struct xrt_vec3 a; - struct xrt_vec3 g; - - a.x = imu_sample->accel_m_s2.x; - a.y = imu_sample->accel_m_s2.y; - a.z = imu_sample->accel_m_s2.z; - - g.x = imu_sample->gyro_rad_secs.x; - g.y = imu_sample->gyro_rad_secs.y; - g.z = imu_sample->gyro_rad_secs.z; - - m_imu_3dof_update(&dx->fusion, imu_sample->timestamp_ns, &a, &g); - - struct xrt_space_relation rel = {0}; - rel.relation_flags = (enum xrt_space_relation_flags)(XRT_SPACE_RELATION_ORIENTATION_VALID_BIT | - XRT_SPACE_RELATION_ORIENTATION_TRACKED_BIT); - rel.pose.orientation = dx->fusion.rot; - - // m_quat_rotate(rel.pose,) - struct xrt_quat rot; - // struct xrt_vec3 x = {0,-1,0}; - - - struct xrt_vec3 x = {0, -1, 0}; - struct xrt_vec3 z = {0, 0, -1}; - - math_quat_from_plus_x_z(&x, &z, &rot); - math_quat_rotate(&dx->fusion.rot, &rot, &rel.pose.orientation); - - - - m_relation_history_push(dx->rh, &rel, imu_sample->timestamp_ns); -} - -int -depthai_3dof_device_found(struct xrt_prober *xp, - struct xrt_prober_device **devices, - size_t device_count, - size_t index, - cJSON *attached_data, - struct xrt_device **out_xdev) -{ - bool should_do = debug_get_bool_option_depthai_3dof(); - - if (!should_do) { - return 0; - } - - bool camera_images = debug_get_bool_option_depthai_3dof_camera_images(); - - struct xrt_frame_context xfctx; - struct xrt_fs *the_fs = NULL; - if (camera_images) { - the_fs = depthai_fs_stereo_grayscale_and_imu(&xfctx); - } else { - the_fs = depthai_fs_just_imu(&xfctx); - } - if (the_fs == NULL) { - return 0; - } - - struct depthai_xdev *dx = U_DEVICE_ALLOCATE(struct depthai_xdev, U_DEVICE_ALLOC_TRACKING_NONE, 1, 0); - dx->log_level = debug_get_log_option_depthai_log(); - - - dx->xfctx = xfctx; - - m_relation_history_create(&dx->rh); - - - dx->base.update_inputs = depthai_3dof_update_inputs; - dx->base.get_tracked_pose = depthai_3dof_get_tracked_pose; - dx->base.get_view_poses = depthai_3dof_get_view_poses; - dx->base.destroy = depthai_3dof_destroy; - dx->base.name = XRT_DEVICE_DEPTHAI; - dx->base.tracking_origin->type = XRT_TRACKING_TYPE_OTHER; - dx->base.tracking_origin->offset = (struct xrt_pose)XRT_POSE_IDENTITY; - dx->base.inputs[0].name = XRT_INPUT_GENERIC_TRACKER_POSE; - dx->base.orientation_tracking_supported = true; - dx->base.position_tracking_supported = true; - dx->base.device_type = XRT_DEVICE_TYPE_GENERIC_TRACKER; - - - - // Print name. - snprintf(dx->base.str, XRT_DEVICE_NAME_LEN, "DepthAI Head Tracker"); - snprintf(dx->base.serial, XRT_DEVICE_NAME_LEN, "DepthAI Head Tracker"); - - - - struct xrt_slam_sinks tmp = {0}; - - u_var_add_root(dx, "DepthAI Head Tracker", 0); - - if (camera_images) { - dx->pretty.push_frame = depthai_pretty_push_frame; - u_var_add_sink_debug(dx, &dx->debug_sink, "Camera view!"); - u_sink_combiner_create(&dx->xfctx, &dx->pretty, &tmp.left, &tmp.right); - } - - m_imu_3dof_init(&dx->fusion, M_IMU_3DOF_USE_GRAVITY_DUR_300MS); - m_imu_3dof_add_vars(&dx->fusion, dx, ""); - - - - dx->imu_sink.push_imu = depthai_receive_imu_sample; - tmp.imu = &dx->imu_sink; - - - xrt_fs_slam_stream_start(the_fs, &tmp); - - - out_xdev[0] = &dx->base; - return 1; -} diff --git a/src/xrt/state_trackers/gui/gui_scene_record.c b/src/xrt/state_trackers/gui/gui_scene_record.c index 79bafe7ff..a1c300707 100644 --- a/src/xrt/state_trackers/gui/gui_scene_record.c +++ b/src/xrt/state_trackers/gui/gui_scene_record.c @@ -207,7 +207,14 @@ create_depthai_stereo(struct camera_window *cw) return; } - cw->camera.xfs = depthai_fs_stereo_grayscale(&cw->camera.xfctx); + struct depthai_slam_startup_settings settings = {0}; + settings.frames_per_second = 60; + settings.half_size_ov9282 = false; + settings.want_cameras = true; + settings.want_imu = false; + + + cw->camera.xfs = depthai_fs_slam(&cw->camera.xfctx, &settings); if (cw->camera.xfs == NULL) { U_LOG_W("Could not create depthai camera!"); return; diff --git a/src/xrt/state_trackers/gui/gui_scene_video.c b/src/xrt/state_trackers/gui/gui_scene_video.c index 242b1b5e4..ac3d7abcd 100644 --- a/src/xrt/state_trackers/gui/gui_scene_video.c +++ b/src/xrt/state_trackers/gui/gui_scene_video.c @@ -73,7 +73,15 @@ create_depthai_stereo(struct video_select *vs) { vs->xfctx = U_TYPED_CALLOC(struct xrt_frame_context); - vs->xfs = depthai_fs_stereo_grayscale(vs->xfctx); + + struct depthai_slam_startup_settings settings = {0}; + settings.frames_per_second = 60; + settings.half_size_ov9282 = false; + settings.want_cameras = true; + settings.want_imu = false; + + + vs->xfs = depthai_fs_slam(vs->xfctx, &settings); if (vs->xfs == NULL) { U_LOG_E("Failed to open DepthAI camera!"); free(vs->xfctx); diff --git a/src/xrt/targets/common/target_lists.c b/src/xrt/targets/common/target_lists.c index 8bcaa49b5..eca249817 100644 --- a/src/xrt/targets/common/target_lists.c +++ b/src/xrt/targets/common/target_lists.c @@ -146,10 +146,6 @@ struct xrt_prober_entry target_entry_list[] = { {HDK_VID, HDK_PID, hdk_found, "OSVR HDK", "osvr"}, #endif // XRT_BUILD_DRIVER_HDK -#ifdef XRT_BUILD_DRIVER_DEPTHAI - {DEPTHAI_VID, DEPTHAI_PID, depthai_3dof_device_found, "DepthAI Device as Head Tracker", "depthai"}, -#endif - #ifdef XRT_BUILD_DRIVER_WMR {MICROSOFT_VID, HOLOLENS_SENSORS_PID, wmr_found, "Microsoft HoloLens Sensors", "wmr"}, {MICROSOFT_VID, WMR_CONTROLLER_PID, wmr_bt_controller_found, "WMR Bluetooth controller", "wmr"},