mirror of
https://gitlab.freedesktop.org/monado/monado.git
synced 2025-01-19 13:18:32 +00:00
d/ht: support DepthAI cameras and Mercury hand tracking
This commit is contained in:
Moses Turner
parent
00be5d0551
commit
870a2ce704
|
|
@ -228,9 +228,16 @@ if(XRT_BUILD_DRIVER_HANDTRACKING)
|
|||
aux_gstreamer
|
||||
ONNXRuntime::ONNXRuntime
|
||||
${OpenCV_LIBRARIES}
|
||||
t_ht_mercury
|
||||
t_ht_old_rgb
|
||||
hand_async
|
||||
)
|
||||
|
||||
if(XRT_BUILD_DRIVER_DEPTHAI)
|
||||
target_sources(drv_ht PRIVATE ht/ht_prober.c)
|
||||
target_link_libraries(drv_ht PRIVATE drv_depthai)
|
||||
endif()
|
||||
|
||||
target_include_directories(drv_ht PRIVATE ${OpenCV_INCLUDE_DIRS} ${EIGEN3_INCLUDE_DIR})
|
||||
list(APPEND ENABLED_DRIVERS ht)
|
||||
|
||||
|
|
|
|||
|
|
@ -8,11 +8,8 @@
|
|||
* @ingroup drv_ht
|
||||
*/
|
||||
|
||||
#include "gstreamer/gst_pipeline.h"
|
||||
#include "gstreamer/gst_sink.h"
|
||||
#include "ht_interface.h"
|
||||
|
||||
#include "../depthai/depthai_interface.h"
|
||||
|
||||
#include "util/u_var.h"
|
||||
#include "xrt/xrt_defines.h"
|
||||
|
|
@ -20,35 +17,29 @@
|
|||
#include "xrt/xrt_frameserver.h"
|
||||
#include "xrt/xrt_prober.h"
|
||||
|
||||
#include "os/os_time.h"
|
||||
#include "os/os_threading.h"
|
||||
|
||||
#include "math/m_api.h"
|
||||
|
||||
#include "util/u_device.h"
|
||||
#include "util/u_frame.h"
|
||||
#include "util/u_sink.h"
|
||||
#include "util/u_format.h"
|
||||
#include "util/u_logging.h"
|
||||
#include "util/u_time.h"
|
||||
#include "util/u_trace_marker.h"
|
||||
#include "util/u_time.h"
|
||||
#include "util/u_json.h"
|
||||
#include "util/u_config_json.h"
|
||||
#include "util/u_debug.h"
|
||||
#include "util/u_sink.h"
|
||||
|
||||
|
||||
// #include "tracking/t_frame_cv_mat_wrapper.hpp"
|
||||
// #include "tracking/t_calibration_opencv.hpp"
|
||||
#include "tracking/t_hand_tracking.h"
|
||||
|
||||
// Save me, Obi-Wan!
|
||||
|
||||
#include "../../tracking/hand/old_rgb/rgb_interface.h"
|
||||
#include "../../tracking/hand/mercury/hg_interface.h"
|
||||
|
||||
#ifdef XRT_BUILD_DRIVER_DEPTHAI
|
||||
#include "../depthai/depthai_interface.h"
|
||||
#endif
|
||||
|
||||
|
||||
#include <cjson/cJSON.h>
|
||||
|
||||
DEBUG_GET_ONCE_LOG_OPTION(ht_log, "HT_LOG", U_LOGGING_WARN)
|
||||
DEBUG_GET_ONCE_BOOL_OPTION(ht_use_old_rgb, "HT_USE_OLD_RGB", false)
|
||||
|
||||
|
||||
#define HT_TRACE(htd, ...) U_LOG_XDEV_IFL_T(&htd->base, htd->log_level, __VA_ARGS__)
|
||||
|
|
@ -63,18 +54,8 @@ struct ht_device
|
|||
{
|
||||
struct xrt_device base;
|
||||
|
||||
struct xrt_tracking_origin tracking_origin; // probably cargo-culted
|
||||
|
||||
enum xrt_format desired_format;
|
||||
|
||||
struct xrt_frame_context xfctx;
|
||||
|
||||
struct xrt_fs *xfs;
|
||||
|
||||
struct xrt_fs_mode mode;
|
||||
|
||||
struct xrt_prober *prober;
|
||||
|
||||
struct t_hand_tracking_sync *sync;
|
||||
struct t_hand_tracking_async *async;
|
||||
|
||||
|
|
@ -157,27 +138,6 @@ userConfigSetDefaults(struct ht_device *htd)
|
|||
}
|
||||
#endif
|
||||
|
||||
static void
|
||||
on_video_device(struct xrt_prober *xp,
|
||||
struct xrt_prober_device *pdev,
|
||||
const char *product,
|
||||
const char *manufacturer,
|
||||
const char *serial,
|
||||
void *ptr)
|
||||
{
|
||||
// Stolen from gui_scene_record
|
||||
|
||||
struct ht_device *htd = (struct ht_device *)ptr;
|
||||
|
||||
// Hardcoded for the Index.
|
||||
if (product != NULL && manufacturer != NULL) {
|
||||
if ((strcmp(product, "3D Camera") == 0) && (strcmp(manufacturer, "Etron Technology, Inc.") == 0)) {
|
||||
xrt_prober_open_video_device(xp, pdev, &htd->xfctx, &htd->xfs);
|
||||
return;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/*!
|
||||
* xrt_device function implementations
|
||||
*/
|
||||
|
|
@ -219,13 +179,14 @@ ht_device_destroy(struct xrt_device *xdev)
|
|||
u_device_free(&htd->base);
|
||||
}
|
||||
|
||||
struct xrt_device *
|
||||
ht_device_create(struct xrt_prober *xp, struct t_stereo_camera_calibration *calib)
|
||||
static struct ht_device *
|
||||
ht_device_create_common(struct t_stereo_camera_calibration *calib,
|
||||
struct xrt_frame_context *xfctx,
|
||||
struct t_hand_tracking_sync *sync)
|
||||
{
|
||||
XRT_TRACE_MARKER();
|
||||
assert(calib != NULL);
|
||||
|
||||
enum u_device_alloc_flags flags = U_DEVICE_ALLOC_NO_FLAGS;
|
||||
enum u_device_alloc_flags flags = U_DEVICE_ALLOC_NO_FLAGS | U_DEVICE_ALLOC_TRACKING_NONE;
|
||||
|
||||
//! @todo 2 hands hardcoded
|
||||
int num_hands = 2;
|
||||
|
|
@ -233,29 +194,17 @@ ht_device_create(struct xrt_prober *xp, struct t_stereo_camera_calibration *cali
|
|||
// Allocate device
|
||||
struct ht_device *htd = U_DEVICE_ALLOCATE(struct ht_device, flags, num_hands, 0);
|
||||
|
||||
// Setup logging first. We like logging.
|
||||
// Setup logging first
|
||||
htd->log_level = debug_get_log_option_ht_log();
|
||||
|
||||
// Set defaults - most people won't have a config json and it won't get past here.
|
||||
htd->desired_format = XRT_FORMAT_YUYV422;
|
||||
htd->xfctx.nodes = xfctx->nodes;
|
||||
|
||||
htd->prober = xp;
|
||||
htd->xfs = NULL;
|
||||
|
||||
xrt_prober_list_video_devices(htd->prober, on_video_device, htd);
|
||||
|
||||
if (htd->xfs == NULL) {
|
||||
return NULL;
|
||||
}
|
||||
|
||||
htd->base.tracking_origin = &htd->tracking_origin;
|
||||
htd->base.tracking_origin->type = XRT_TRACKING_TYPE_RGB;
|
||||
htd->base.tracking_origin->offset.position.x = 0.0f;
|
||||
htd->base.tracking_origin->offset.position.y = 0.0f;
|
||||
htd->base.tracking_origin->offset.position.z = 0.0f;
|
||||
htd->base.tracking_origin->offset.orientation.w = 1.0f;
|
||||
|
||||
|
||||
htd->base.update_inputs = ht_device_update_inputs;
|
||||
htd->base.get_hand_tracking = ht_device_get_hand_tracking;
|
||||
htd->base.destroy = ht_device_destroy;
|
||||
|
|
@ -273,34 +222,91 @@ ht_device_create(struct xrt_prober *xp, struct t_stereo_camera_calibration *cali
|
|||
htd->base.position_tracking_supported = true;
|
||||
htd->base.hand_tracking_supported = true;
|
||||
|
||||
htd->sync = t_hand_tracking_sync_old_rgb_create(calib);
|
||||
htd->async = t_hand_tracking_async_default_create(&htd->xfctx, htd->sync);
|
||||
htd->sync = sync;
|
||||
|
||||
htd->async = t_hand_tracking_async_default_create(&htd->xfctx, sync);
|
||||
return htd;
|
||||
}
|
||||
|
||||
struct index_camera_finder
|
||||
{
|
||||
struct xrt_fs *xfs;
|
||||
struct xrt_frame_context xfctx;
|
||||
bool found;
|
||||
};
|
||||
|
||||
static void
|
||||
on_video_device(struct xrt_prober *xp,
|
||||
struct xrt_prober_device *pdev,
|
||||
const char *product,
|
||||
const char *manufacturer,
|
||||
const char *serial,
|
||||
void *ptr)
|
||||
{
|
||||
struct index_camera_finder *finder = (struct index_camera_finder *)ptr;
|
||||
|
||||
// Hardcoded for the Index.
|
||||
if (product != NULL && manufacturer != NULL) {
|
||||
if ((strcmp(product, "3D Camera") == 0) && (strcmp(manufacturer, "Etron Technology, Inc.") == 0)) {
|
||||
xrt_prober_open_video_device(xp, pdev, &finder->xfctx, &finder->xfs);
|
||||
return;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
struct xrt_device *
|
||||
ht_device_create_index(struct xrt_prober *xp, struct t_stereo_camera_calibration *calib)
|
||||
{
|
||||
XRT_TRACE_MARKER();
|
||||
assert(calib != NULL);
|
||||
|
||||
|
||||
struct index_camera_finder finder = {0};
|
||||
|
||||
xrt_prober_list_video_devices(xp, on_video_device, &finder);
|
||||
|
||||
|
||||
if (finder.xfs == NULL) {
|
||||
return NULL;
|
||||
}
|
||||
|
||||
bool use_old_rgb = debug_get_bool_option_ht_use_old_rgb();
|
||||
|
||||
struct t_hand_tracking_sync *sync;
|
||||
|
||||
if (use_old_rgb) {
|
||||
sync = t_hand_tracking_sync_old_rgb_create(calib);
|
||||
} else {
|
||||
sync = t_hand_tracking_sync_mercury_create(calib, MERCURY_OUTPUT_SPACE_LEFT_CAMERA);
|
||||
}
|
||||
|
||||
struct ht_device *htd = ht_device_create_common(calib, &finder.xfctx, sync);
|
||||
|
||||
struct xrt_frame_sink *tmp = NULL;
|
||||
|
||||
u_sink_stereo_sbs_to_slam_sbs_create(&htd->xfctx, &htd->async->left, &htd->async->right, &tmp);
|
||||
|
||||
// Converts images (we'd expect YUV422 or MJPEG) to R8G8B8. Can take a long time, especially on unoptimized
|
||||
// builds. If it's really slow, triple-check that you built Monado with optimizations!
|
||||
//!@todo
|
||||
u_sink_create_format_converter(&htd->xfctx, XRT_FORMAT_R8G8B8, tmp, &tmp);
|
||||
if (use_old_rgb) {
|
||||
u_sink_create_format_converter(&htd->xfctx, XRT_FORMAT_R8G8B8, tmp, &tmp);
|
||||
} else {
|
||||
u_sink_create_format_converter(&htd->xfctx, XRT_FORMAT_L8, tmp, &tmp);
|
||||
}
|
||||
|
||||
// This puts u_sink_create_to_r8g8b8_or_l8 on its own thread, so that nothing gets backed up if it runs slower
|
||||
// This puts the format converter on its own thread, so that nothing gets backed up if it runs slower
|
||||
// than the native camera framerate.
|
||||
u_sink_simple_queue_create(&htd->xfctx, tmp, &tmp);
|
||||
|
||||
struct xrt_fs_mode *modes;
|
||||
struct xrt_fs_mode *modes = NULL;
|
||||
uint32_t count;
|
||||
|
||||
xrt_fs_enumerate_modes(htd->xfs, &modes, &count);
|
||||
xrt_fs_enumerate_modes(finder.xfs, &modes, &count);
|
||||
|
||||
// Index should only have XRT_FORMAT_YUYV422 or XRT_FORMAT_MJPEG.
|
||||
|
||||
bool found_mode = false;
|
||||
uint32_t selected_mode = 0;
|
||||
|
||||
for (; selected_mode < count; selected_mode++) {
|
||||
if (modes[selected_mode].format == htd->desired_format) {
|
||||
if (modes[selected_mode].format == XRT_FORMAT_YUYV422) {
|
||||
found_mode = true;
|
||||
break;
|
||||
}
|
||||
|
|
@ -313,9 +319,49 @@ ht_device_create(struct xrt_prober *xp, struct t_stereo_camera_calibration *cali
|
|||
|
||||
free(modes);
|
||||
|
||||
xrt_fs_stream_start(htd->xfs, tmp, XRT_FS_CAPTURE_TYPE_TRACKING, selected_mode);
|
||||
xrt_fs_stream_start(finder.xfs, tmp, XRT_FS_CAPTURE_TYPE_TRACKING, selected_mode);
|
||||
|
||||
HT_DEBUG(htd, "Hand Tracker initialized!");
|
||||
|
||||
return &htd->base;
|
||||
}
|
||||
|
||||
#ifdef XRT_BUILD_DRIVER_DEPTHAI
|
||||
struct xrt_device *
|
||||
ht_device_create_depthai_ov9282()
|
||||
{
|
||||
XRT_TRACE_MARKER();
|
||||
|
||||
struct xrt_frame_context xfctx = {0};
|
||||
|
||||
struct xrt_fs *xfs = depthai_fs_stereo_grayscale(&xfctx);
|
||||
|
||||
if (xfs == NULL) {
|
||||
return NULL;
|
||||
}
|
||||
|
||||
struct t_stereo_camera_calibration *calib = NULL;
|
||||
|
||||
depthai_fs_get_stereo_calibration(xfs, &calib);
|
||||
|
||||
assert(calib != NULL);
|
||||
|
||||
struct t_hand_tracking_sync *sync;
|
||||
|
||||
sync = t_hand_tracking_sync_mercury_create(calib, MERCURY_OUTPUT_SPACE_LEFT_CAMERA);
|
||||
|
||||
struct ht_device *htd = ht_device_create_common(calib, &xfctx, sync);
|
||||
|
||||
struct xrt_slam_sinks tmp;
|
||||
|
||||
t_stereo_camera_calibration_reference(&calib, NULL);
|
||||
|
||||
u_sink_force_genlock_create(&htd->xfctx, &htd->async->left, &htd->async->right, &tmp.left, &tmp.right);
|
||||
|
||||
xrt_fs_slam_stream_start(xfs, &tmp);
|
||||
|
||||
HT_DEBUG(htd, "Hand Tracker initialized!");
|
||||
|
||||
return &htd->base;
|
||||
}
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -11,6 +11,7 @@
|
|||
#pragma once
|
||||
|
||||
#include "xrt/xrt_device.h"
|
||||
#include "xrt/xrt_config_drivers.h"
|
||||
|
||||
#include "tracking/t_tracking.h"
|
||||
#include "xrt/xrt_prober.h"
|
||||
|
|
@ -32,7 +33,15 @@ extern "C" {
|
|||
* @ingroup drv_ht
|
||||
*/
|
||||
struct xrt_device *
|
||||
ht_device_create(struct xrt_prober *xp, struct t_stereo_camera_calibration *calib);
|
||||
ht_device_create_index(struct xrt_prober *xp, struct t_stereo_camera_calibration *calib);
|
||||
|
||||
#ifdef XRT_BUILD_DRIVER_DEPTHAI
|
||||
struct xrt_device *
|
||||
ht_device_create_depthai_ov9282(void);
|
||||
|
||||
struct xrt_auto_prober *
|
||||
ht_create_auto_prober();
|
||||
#endif
|
||||
|
||||
/*!
|
||||
* @dir drivers/ht
|
||||
|
|
|
|||
57
src/xrt/drivers/ht/ht_prober.c
Normal file
57
src/xrt/drivers/ht/ht_prober.c
Normal file
|
|
@ -0,0 +1,57 @@
|
|||
// Copyright 2021, Collabora, Ltd.
|
||||
// SPDX-License-Identifier: BSL-1.0
|
||||
/*!
|
||||
* @file
|
||||
* @brief Hack for making it easy to run our tracking with DepthAI cameras.
|
||||
* @author Moses Turner <moses@collabora.com>
|
||||
* @ingroup drv_ht
|
||||
*/
|
||||
|
||||
#include "ht_interface.h"
|
||||
|
||||
#include "util/u_debug.h"
|
||||
|
||||
|
||||
DEBUG_GET_ONCE_BOOL_OPTION(ht_use_depthai, "HT_USE_DEPTHAI", false)
|
||||
|
||||
static void
|
||||
ht_prober_destroy(struct xrt_auto_prober *p)
|
||||
{
|
||||
free(p);
|
||||
}
|
||||
|
||||
static int
|
||||
ht_prober_autoprobe(struct xrt_auto_prober *xap,
|
||||
cJSON *attached_data,
|
||||
bool no_hmds,
|
||||
struct xrt_prober *xp,
|
||||
struct xrt_device **out_xdevs)
|
||||
{
|
||||
if (!debug_get_bool_option_ht_use_depthai()) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
struct xrt_device *ht = ht_device_create_depthai_ov9282();
|
||||
|
||||
if (ht == NULL) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
int out_idx = 0;
|
||||
|
||||
out_xdevs[out_idx++] = ht;
|
||||
return out_idx;
|
||||
}
|
||||
|
||||
|
||||
|
||||
struct xrt_auto_prober *
|
||||
ht_create_auto_prober()
|
||||
{
|
||||
struct xrt_auto_prober *xap = U_TYPED_CALLOC(struct xrt_auto_prober);
|
||||
xap->name = "ht_depthai";
|
||||
xap->destroy = ht_prober_destroy;
|
||||
xap->lelo_dallas_autoprobe = ht_prober_autoprobe;
|
||||
|
||||
return xap;
|
||||
}
|
||||
|
|
@ -1411,7 +1411,7 @@ survive_device_autoprobe(struct xrt_auto_prober *xap,
|
|||
struct xrt_pose head_in_left_cam;
|
||||
vive_get_stereo_camera_calibration(&ss->hmd->hmd.config, &cal, &head_in_left_cam);
|
||||
|
||||
struct xrt_device *ht = ht_device_create(xp, cal);
|
||||
struct xrt_device *ht = ht_device_create_index(xp, cal);
|
||||
if (ht != NULL) { // Returns NULL if there's a problem and the hand tracker can't start. By no means a
|
||||
// fatal error.
|
||||
struct xrt_device *wrap =
|
||||
|
|
|
|||
|
|
@ -269,7 +269,7 @@ init_valve_index(struct xrt_prober *xp,
|
|||
// vive_get_stereo_camera_calibration(&ss->hmd->hmd.config, &cal, &head_in_left_cam);
|
||||
vive_get_stereo_camera_calibration(&d->config, &cal, &head_in_left_cam);
|
||||
|
||||
struct xrt_device *ht = ht_device_create(xp, cal);
|
||||
struct xrt_device *ht = ht_device_create_index(xp, cal);
|
||||
if (ht != NULL) { // Returns NULL if there's a problem and the hand tracker can't start. By no means a
|
||||
// fatal error.
|
||||
struct xrt_device *wrap =
|
||||
|
|
|
|||
|
|
@ -83,6 +83,12 @@
|
|||
#include "euroc/euroc_interface.h"
|
||||
#endif
|
||||
|
||||
#ifdef XRT_BUILD_DRIVER_HANDTRACKING
|
||||
#ifdef XRT_BUILD_DRIVER_DEPTHAI
|
||||
#include "ht/ht_interface.h"
|
||||
#endif
|
||||
#endif
|
||||
|
||||
|
||||
/*!
|
||||
* Each entry should be a vendor ID (VID), product ID (PID), a "found" function,
|
||||
|
|
@ -197,6 +203,11 @@ xrt_auto_prober_creator target_auto_list[] = {
|
|||
dummy_create_auto_prober,
|
||||
#endif
|
||||
|
||||
#ifdef XRT_BUILD_DRIVER_HANDTRACKING
|
||||
#ifdef XRT_BUILD_DRIVER_DEPTHAI
|
||||
ht_create_auto_prober,
|
||||
#endif
|
||||
#endif
|
||||
NULL, // Terminate
|
||||
};
|
||||
|
||||
|
|
|
|||
Loading…
Reference in a new issue