mirrors/monado
1
0
Fork 0
mirror of https://gitlab.freedesktop.org/monado/monado.git synced 2024-12-28 18:46:18 +00:00
Code Issues Actions Packages Projects Releases Wiki Activity

d/dai: Update DepthAI driver

Browse source 
* 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
This commit is contained in:
Moses Turner 2022-09-21 06:27:38 -05:00
parent 201708dee5
commit 6fdd790da0
7 changed files with 73 additions and 315 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

5
src/xrt/drivers/CMakeLists.txt
View file

@ -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

75
src/xrt/drivers/depthai/depthai_driver.cpp
View file

@ -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<dai::IMUData> imuData = depthai->imu_queue->get<dai::IMUData>();
if (depthai->first_frames_idx < debug_get_num_option_depthai_startup_wait_frames()) {
return;
}
std::vector<dai::IMUPacket> 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)

31
src/xrt/drivers/depthai/depthai_interface.h
View file

@ -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
/*!

254
src/xrt/drivers/depthai/depthai_tracked_device.c
View file

@ -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 <moses@collabora.com>
* @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 <stdio.h>
#include <assert.h>
#include <unistd.h>
#include <pthread.h>
#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;
}

9
src/xrt/state_trackers/gui/gui_scene_record.c
View file

@ -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;

10
src/xrt/state_trackers/gui/gui_scene_video.c
View file

@ -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);

4
src/xrt/targets/common/target_lists.c
View file

@ -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"},