From 9d9b22a76545e8c71f3bad08a22fc6d5dd404cb9 Mon Sep 17 00:00:00 2001 From: Pete Date: Sat, 18 Jan 2020 07:32:32 +1300 Subject: [PATCH] initial work to make the leap motion work as a tracking camera --- src/xrt/auxiliary/CMakeLists.txt | 1 + src/xrt/auxiliary/tracking/t_calibration.cpp | 30 +- src/xrt/auxiliary/tracking/t_tracker_leap.cpp | 304 ++++++++++++++++++ src/xrt/auxiliary/tracking/t_tracking.h | 11 + src/xrt/auxiliary/util/u_format.c | 5 + src/xrt/auxiliary/util/u_sink_converter.c | 39 +++ src/xrt/drivers/v4l2/v4l2_driver.c | 19 +- src/xrt/include/xrt/xrt_defines.h | 3 + src/xrt/include/xrt/xrt_tracking.h | 64 ++++ .../state_trackers/gui/gui_scene_calibrate.c | 4 +- 10 files changed, 475 insertions(+), 5 deletions(-) create mode 100644 src/xrt/auxiliary/tracking/t_tracker_leap.cpp diff --git a/src/xrt/auxiliary/CMakeLists.txt b/src/xrt/auxiliary/CMakeLists.txt index da034657c..f457283c0 100644 --- a/src/xrt/auxiliary/CMakeLists.txt +++ b/src/xrt/auxiliary/CMakeLists.txt @@ -48,6 +48,7 @@ if(BUILD_TRACKING) tracking/t_tracker_psmv_fusion.hpp tracking/t_tracker_psmv.cpp tracking/t_tracker_psvr.cpp + tracking/t_tracker_leap.cpp ) endif() diff --git a/src/xrt/auxiliary/tracking/t_calibration.cpp b/src/xrt/auxiliary/tracking/t_calibration.cpp index d620bdbbe..07471ccc2 100644 --- a/src/xrt/auxiliary/tracking/t_calibration.cpp +++ b/src/xrt/auxiliary/tracking/t_calibration.cpp @@ -190,7 +190,15 @@ ensure_buffers_are_allocated(class Calibration &c, int rows, int cols) return; } - c.gray = cv::Mat(rows, cols, CV_8UC1, cv::Scalar(0)); + // if our rgb is not alloced but our gray already is, alloc our rgb now + // - we will hit this path if we receive L8 format. + if (c.gray.cols == cols && c.gray.rows == rows) { + c.gui.rgb = cv::Mat(rows, cols, CV_8UC3, cv::Scalar(0)); + refresh_gui_frame(c, rows, cols); + return; + } + + c.gray = cv::Mat(rows, cols, CV_8UC1, cv::Scalar(0)); refresh_gui_frame(c, rows, cols); } @@ -1037,6 +1045,21 @@ make_remap_view(class Calibration &c, struct xrt_frame *xf) * */ +XRT_NO_INLINE static void +process_frame_l8(class Calibration &c, struct xrt_frame *xf) +{ + + int w = (int)xf->width; + int h = (int)xf->height; + + cv::Mat data(h, w, CV_8UC1, xf->data, xf->stride); + c.gray = data; + ensure_buffers_are_allocated(c, data.rows, data.cols); + c.gui.frame->source_sequence = xf->source_sequence; + + cv::cvtColor(data, c.gui.rgb, cv::COLOR_GRAY2RGB); +} + XRT_NO_INLINE static void process_frame_yuv(class Calibration &c, struct xrt_frame *xf) { @@ -1149,6 +1172,7 @@ t_calibration_frame(struct xrt_frame_sink *xsink, struct xrt_frame *xf) switch (xf->format) { case XRT_FORMAT_YUV888: process_frame_yuv(c, xf); break; case XRT_FORMAT_YUV422: process_frame_yuyv(c, xf); break; + case XRT_FORMAT_L8: process_frame_l8(c, xf); break; default: P("ERROR: Bad format '%s'", u_format_str(xf->format)); make_gui_str(c); @@ -1255,7 +1279,9 @@ t_calibration_stereo_create(struct xrt_frame_context *xfctx, } // Ensure we only get yuv or yuyv frames. - u_sink_create_to_yuv_or_yuyv(xfctx, *out_sink, out_sink); + //u_sink_create_to_yuv_or_yuyv(xfctx, *out_sink, out_sink); + u_sink_create_to_r8g8b8_or_l8(xfctx, *out_sink, out_sink); + // Build the board model. build_board_position(c); diff --git a/src/xrt/auxiliary/tracking/t_tracker_leap.cpp b/src/xrt/auxiliary/tracking/t_tracker_leap.cpp new file mode 100644 index 000000000..b486ff84a --- /dev/null +++ b/src/xrt/auxiliary/tracking/t_tracker_leap.cpp @@ -0,0 +1,304 @@ +// Copyright 2019, Collabora, Ltd. +// SPDX-License-Identifier: BSL-1.0 +/*! + * @file + * @brief Leap Motion tracker code. + * @author Pete Black + * @ingroup aux_tracking + */ + +#include "xrt/xrt_tracking.h" + +#include "tracking/t_tracking.h" + +#include "util/u_misc.h" +#include "util/u_debug.h" +#include "util/u_frame.h" +#include "util/u_format.h" + +#include "math/m_api.h" + +#include "os/os_threading.h" + +#include +#include +#include + + +class TrackerLeap +{ +public: + struct xrt_tracked_leap base = {}; + struct xrt_frame_sink sink = {}; + struct xrt_frame_node node = {}; + + //! Frame waiting to be processed. + struct xrt_frame *frame; + + //! Thread and lock helper. + struct os_thread_helper oth; + + //! Have we received a new IMU sample. + bool has_imu = false; + + timepoint_ns last_imu{0}; + + struct + { + struct xrt_vec3 pos = {}; + struct xrt_quat rot = {}; + } fusion; +}; + +static void +procces(TrackerLeap &t, struct xrt_frame *xf) +{ + // Only IMU data + if (xf == NULL) { + return; + } + + xrt_frame_reference(&xf, NULL); +} + +static void +run(TrackerLeap &t) +{ + struct xrt_frame *frame = NULL; + + os_thread_helper_lock(&t.oth); + + while (os_thread_helper_is_running_locked(&t.oth)) { + // No data + if (!t.has_imu || t.frame == NULL) { + os_thread_helper_wait_locked(&t.oth); + } + + if (!os_thread_helper_is_running_locked(&t.oth)) { + break; + } + + // Take a reference on the current frame, this keeps it alive + // if it is replaced during the consumer processing it, but + // we no longer need to hold onto the frame on the queue we + // just move the pointer. + frame = t.frame; + t.frame = NULL; + + // Unlock the mutex when we do the work. + os_thread_helper_unlock(&t.oth); + + procces(t, frame); + + // Have to lock it again. + os_thread_helper_lock(&t.oth); + } + + os_thread_helper_unlock(&t.oth); +} + +static void +get_pose(TrackerLeap &t, + struct time_state *timestate, + timepoint_ns when_ns, + struct xrt_space_relation *out_relation) +{ + os_thread_helper_lock(&t.oth); + + // Don't do anything if we have stopped. + if (!os_thread_helper_is_running_locked(&t.oth)) { + os_thread_helper_unlock(&t.oth); + return; + } + + out_relation->pose.position = t.fusion.pos; + out_relation->pose.orientation = t.fusion.rot; + + //! @todo assuming that orientation is actually currently tracked. + out_relation->relation_flags = (enum xrt_space_relation_flags)( + XRT_SPACE_RELATION_POSITION_VALID_BIT | + XRT_SPACE_RELATION_POSITION_TRACKED_BIT | + XRT_SPACE_RELATION_ORIENTATION_VALID_BIT | + XRT_SPACE_RELATION_ORIENTATION_TRACKED_BIT); + + os_thread_helper_unlock(&t.oth); +} + +static void +imu_data(TrackerLeap &t, + timepoint_ns timestamp_ns, + struct xrt_tracking_sample *sample) +{ + os_thread_helper_lock(&t.oth); + + // Don't do anything if we have stopped. + if (!os_thread_helper_is_running_locked(&t.oth)) { + os_thread_helper_unlock(&t.oth); + return; + } + if (t.last_imu != 0) { + time_duration_ns delta_ns = timestamp_ns - t.last_imu; + float dt = time_ns_to_s(delta_ns); + // Super simple fusion. + math_quat_integrate_velocity( + &t.fusion.rot, &sample->gyro_rad_secs, dt, &t.fusion.rot); + } + t.last_imu = timestamp_ns; + + os_thread_helper_unlock(&t.oth); +} + +static void +frame(TrackerLeap &t, struct xrt_frame *xf) +{ + os_thread_helper_lock(&t.oth); + + // Don't do anything if we have stopped. + if (!os_thread_helper_is_running_locked(&t.oth)) { + os_thread_helper_unlock(&t.oth); + return; + } + + xrt_frame_reference(&t.frame, xf); + + // Wake up the thread. + os_thread_helper_signal_locked(&t.oth); + + os_thread_helper_unlock(&t.oth); +} + +static void +break_apart(TrackerLeap &t) +{ + os_thread_helper_stop(&t.oth); +} + + +/* + * + * C wrapper functions. + * + */ + +extern "C" void +t_leap_push_imu(struct xrt_tracked_leap *xtleap, + timepoint_ns timestamp_ns, + struct xrt_tracking_sample *sample) +{ + auto &t = *container_of(xtleap, TrackerLeap, base); + imu_data(t, timestamp_ns, sample); +} + +extern "C" void +t_leap_get_tracked_pose(struct xrt_tracked_leap *xtleap, + struct time_state *timestate, + timepoint_ns when_ns, + struct xrt_space_relation *out_relation) +{ + auto &t = *container_of(xtleap, TrackerLeap, base); + get_pose(t, timestate, when_ns, out_relation); +} + +extern "C" void +t_leap_fake_destroy(struct xrt_tracked_leap *xtleap) +{ + auto &t = *container_of(xtleap, TrackerLeap, base); + (void)t; + // Not the real destroy function +} + +extern "C" void +t_leap_sink_push_frame(struct xrt_frame_sink *xsink, struct xrt_frame *xf) +{ + auto &t = *container_of(xsink, TrackerLeap, sink); + frame(t, xf); +} + +extern "C" void +t_leap_node_break_apart(struct xrt_frame_node *node) +{ + auto &t = *container_of(node, TrackerLeap, node); + break_apart(t); +} + +extern "C" void +t_leap_node_destroy(struct xrt_frame_node *node) +{ + auto t_ptr = container_of(node, TrackerLeap, node); + + os_thread_helper_destroy(&t_ptr->oth); + + delete t_ptr; +} + +extern "C" void * +t_leap_run(void *ptr) +{ + auto &t = *(TrackerLeap *)ptr; + run(t); + return NULL; +} + + +/* + * + * Exported functions. + * + */ + +extern "C" int +t_leap_start(struct xrt_tracked_psvr *xtleap) +{ + auto &t = *container_of(xtleap, TrackerLeap, base); + int ret; + + ret = os_thread_helper_start(&t.oth, t_leap_run, &t); + if (ret != 0) { + return ret; + } + + return ret; +} + +extern "C" int +t_leap_create(struct xrt_frame_context *xfctx, + struct t_stereo_camera_calibration *data, + struct xrt_tracked_leap **out_xtleap, + struct xrt_frame_sink **out_sink) +{ + fprintf(stderr, "%s\n", __func__); + + auto &t = *(new TrackerLeap()); + int ret; + + t.base.get_tracked_pose = t_leap_get_tracked_pose; + t.base.push_imu = t_leap_push_imu; + t.base.destroy = t_leap_fake_destroy; + t.sink.push_frame = t_leap_sink_push_frame; + t.node.break_apart = t_leap_node_break_apart; + t.node.destroy = t_leap_node_destroy; + t.fusion.rot.w = 1.0f; + + ret = os_thread_helper_init(&t.oth); + if (ret != 0) { + delete (&t); + return ret; + } + + // HACK, to counter the tracking origin offset. + t.fusion.pos.x = 0.0f; + t.fusion.pos.y = 0.6f; + t.fusion.pos.z = -2.0f; + + t.fusion.rot.x = 0.0f; + t.fusion.rot.y = 1.0f; + t.fusion.rot.z = 0.0f; + t.fusion.rot.w = 0.0f; + + xrt_frame_context_add(xfctx, &t.node); + + *out_sink = &t.sink; + *out_xtleap = &t.base; + + return 0; +} diff --git a/src/xrt/auxiliary/tracking/t_tracking.h b/src/xrt/auxiliary/tracking/t_tracking.h index dac7112f3..c2d6b2bd2 100644 --- a/src/xrt/auxiliary/tracking/t_tracking.h +++ b/src/xrt/auxiliary/tracking/t_tracking.h @@ -70,6 +70,7 @@ extern "C" { struct xrt_tracked_psmv; struct xrt_tracked_psvr; +struct xrt_tracked_leap; /* @@ -292,6 +293,16 @@ t_psvr_create(struct xrt_frame_context *xfctx, struct xrt_tracked_psvr **out_xtvr, struct xrt_frame_sink **out_sink); +int +t_leap_start(struct xrt_tracked_psvr *xtvr); + +int +t_leap_create(struct xrt_frame_context *xfctx, + struct t_stereo_camera_calibration *data, + struct xrt_tracked_leap **out_xtleap, + struct xrt_frame_sink **out_sink); + + /* * diff --git a/src/xrt/auxiliary/util/u_format.c b/src/xrt/auxiliary/util/u_format.c index 0b1d11a98..ea2580bea 100644 --- a/src/xrt/auxiliary/util/u_format.c +++ b/src/xrt/auxiliary/util/u_format.c @@ -22,6 +22,7 @@ u_format_str(enum xrt_format f) case XRT_FORMAT_R8G8: return "XRT_FORMAT_R8G8"; case XRT_FORMAT_R8: return "XRT_FORMAT_R8"; case XRT_FORMAT_L8: return "XRT_FORMAT_L8"; + case XRT_FORMAT_L8_LEAP: return "XRT_FORMAT_L8_LEAP"; case XRT_FORMAT_BITMAP_8X1: return "XRT_FORMAT_BITMAP_8X1"; case XRT_FORMAT_BITMAP_8X8: return "XRT_FORMAT_BITMAP_8X8"; case XRT_FORMAT_YUV888: return "XRT_FORMAT_YUV888"; @@ -41,6 +42,7 @@ u_format_is_blocks(enum xrt_format f) case XRT_FORMAT_R8G8: case XRT_FORMAT_R8: case XRT_FORMAT_L8: + case XRT_FORMAT_L8_LEAP: case XRT_FORMAT_BITMAP_8X1: case XRT_FORMAT_BITMAP_8X8: case XRT_FORMAT_YUV888: @@ -68,6 +70,7 @@ u_format_block_width(enum xrt_format f) // Regular one pixel per block formats. return 1; case XRT_FORMAT_YUV422: + case XRT_FORMAT_L8_LEAP: // Two pixels per block. return 2; case XRT_FORMAT_BITMAP_8X8: @@ -88,6 +91,7 @@ u_format_block_height(enum xrt_format f) case XRT_FORMAT_R8G8: case XRT_FORMAT_R8: case XRT_FORMAT_L8: + case XRT_FORMAT_L8_LEAP: case XRT_FORMAT_BITMAP_8X1: case XRT_FORMAT_YUV888: case XRT_FORMAT_YUV422: @@ -110,6 +114,7 @@ u_format_block_size(enum xrt_format f) // One byte blocks return 1; case XRT_FORMAT_R8G8: + case XRT_FORMAT_L8_LEAP: // Two bytes, 16bits. return 2; case XRT_FORMAT_R8G8B8: diff --git a/src/xrt/auxiliary/util/u_sink_converter.c b/src/xrt/auxiliary/util/u_sink_converter.c index 52c7e6e58..e06897084 100644 --- a/src/xrt/auxiliary/util/u_sink_converter.c +++ b/src/xrt/auxiliary/util/u_sink_converter.c @@ -159,6 +159,39 @@ from_YUV422_to_R8G8B8(struct u_sink_converter *s, } +//LEAP Motion interleaved L8 format + +inline static void +L8LEAP_to_L8(const uint8_t *input, uint32_t *l8a, uint32_t *l8b) +{ + *l8a = input[0]; + *l8b = input[1]; +} + +static void +from_L8LEAP_to_L8(struct u_sink_converter *s, + uint32_t w, + uint32_t h, + size_t stride, + const uint8_t *data) +{ + for (uint32_t y = 0; y < h; y++) { + for (uint32_t x = 0; x < w; x += 2) { + const uint8_t *src = data; + uint8_t *dst = s->frame->data; + uint8_t *dst2 = s->frame->data + s->frame->stride/2 * s->frame->height; + + src = src + (y * stride) + (x * 2); + dst = dst + (y * s->frame->stride/2) + x; + dst2 = dst2 + (y * s->frame->stride/2) + x; + L8LEAP_to_L8(src, dst,dst2); + } + } +} + + + + /* * * MJPEG @@ -316,6 +349,12 @@ receive_frame_r8g8b8_or_l8(struct xrt_frame_sink *xs, struct xrt_frame *xf) case XRT_FORMAT_R8G8B8: s->downstream->push_frame(s->downstream, xf); return; + case XRT_FORMAT_L8_LEAP: + ensure_data(s, XRT_FORMAT_L8, xf->width * 2, xf->height); + from_L8LEAP_to_L8(s, xf->width, xf->height, xf->stride, + xf->data); + xf->width *=2; + break; case XRT_FORMAT_YUV422: ensure_data(s, XRT_FORMAT_R8G8B8, xf->width, xf->height); from_YUV422_to_R8G8B8(s, xf->width, xf->height, xf->stride, diff --git a/src/xrt/drivers/v4l2/v4l2_driver.c b/src/xrt/drivers/v4l2/v4l2_driver.c index 766b26478..91dca8351 100644 --- a/src/xrt/drivers/v4l2/v4l2_driver.c +++ b/src/xrt/drivers/v4l2/v4l2_driver.c @@ -133,6 +133,7 @@ struct v4l2_fs struct { bool ps4_cam; + bool leap_motion; } quirks; struct v4l2_frame frames[NUM_V4L2_BUFFERS]; @@ -322,7 +323,10 @@ v4l2_query_cap_and_validate(struct v4l2_fs *vid) vid->quirks.ps4_cam = strcmp(card, "USB Camera-OV580: USB Camera-OV") == 0; - if (vid->quirks.ps4_cam) { + vid->quirks.leap_motion = + strcmp(card, "Leap Motion Controller") == 0; + + if (vid->quirks.ps4_cam) { // The experimented best controls to best track things. v4l2_add_control_state(vid, V4L2_CID_GAIN, 0, 2, "gain"); v4l2_add_control_state(vid, V4L2_CID_AUTO_WHITE_BALANCE, 0, 2, @@ -335,7 +339,7 @@ v4l2_query_cap_and_validate(struct v4l2_fs *vid) vid, V4L2_CID_EXPOSURE_ABSOLUTE, debug_get_num_option_v4l2_exposure_absolute(), 2, "exposure_absolute"); - } + } // Done return 0; @@ -435,6 +439,13 @@ v4l2_quirk_apply_ps4(struct v4l2_fs *vid, struct v4l2_source_descriptor *desc) } } +static void +v4l2_quirk_apply_leap(struct v4l2_fs *vid, struct v4l2_source_descriptor *desc) +{ + desc->base.format = XRT_FORMAT_L8_LEAP; + desc->base.stereo_format = XRT_STEREO_FORMAT_SBS; +} + static struct v4l2_source_descriptor * v4l2_add_descriptor(struct v4l2_fs *vid) { @@ -534,6 +545,10 @@ v4l2_list_modes_size(struct v4l2_fs *vid, if (vid->quirks.ps4_cam) { v4l2_quirk_apply_ps4(vid, desc); } + + if (vid->quirks.leap_motion) { + v4l2_quirk_apply_leap(vid, desc); + } } static void diff --git a/src/xrt/include/xrt/xrt_defines.h b/src/xrt/include/xrt/xrt_defines.h index dec46511c..24a9c93b1 100644 --- a/src/xrt/include/xrt/xrt_defines.h +++ b/src/xrt/include/xrt/xrt_defines.h @@ -66,6 +66,7 @@ enum xrt_format XRT_FORMAT_R8, XRT_FORMAT_L8, // Luminence, R = L, G = L, B = L. + XRT_FORMAT_L8_LEAP, // interleaved mono format used by Leap Motion XRT_FORMAT_BITMAP_8X1, // One bit format tiled in 8x1 blocks. XRT_FORMAT_BITMAP_8X8, // One bit format tiled in 8X8 blocks. @@ -74,6 +75,8 @@ enum xrt_format XRT_FORMAT_YUV422, XRT_FORMAT_MJPEG, + + }; /*! diff --git a/src/xrt/include/xrt/xrt_tracking.h b/src/xrt/include/xrt/xrt_tracking.h index 59e1b5db1..47c8006ad 100644 --- a/src/xrt/include/xrt/xrt_tracking.h +++ b/src/xrt/include/xrt/xrt_tracking.h @@ -174,6 +174,40 @@ struct xrt_tracked_psvr }; +/*! + * A Leap Motion tracker - notional single tracked object + */ +struct xrt_tracked_leap +{ + //! The tracking system origin for this object. + struct xrt_tracking_origin *origin; + + //! Device owning this object. + struct xrt_device *xdev; + + /*! + * Push a IMU sample into the tracking system. + */ + void (*push_imu)(struct xrt_tracked_leap *, + timepoint_ns timestamp_ns, + struct xrt_tracking_sample *sample); + + /*! + * Called by the owning @ref xrt_device @ref xdev to get the pose of + * the object in the tracking space at the given time. + */ + void (*get_tracked_pose)(struct xrt_tracked_leap *, + struct time_state *timekeeper, + timepoint_ns when_ns, + struct xrt_space_relation *out_relation); + + /*! + * Destroy this tracked leap. + */ + void (*destroy)(struct xrt_tracked_leap *); +}; + + /* * * Helper functions. @@ -240,6 +274,36 @@ xrt_tracked_psvr_destroy(struct xrt_tracked_psvr **xtvr_ptr) } +static inline void +xrt_tracked_leap_get_tracked_pose(struct xrt_tracked_leap *leap, + struct time_state *timekeeper, + timepoint_ns when_ns, + struct xrt_space_relation *out_relation) +{ + leap->get_tracked_pose(leap, timekeeper, when_ns, out_relation); +} + +static inline void +xrt_tracked_leap_push_imu(struct xrt_tracked_leap *leap, + timepoint_ns timestamp_ns, + struct xrt_tracking_sample *sample) +{ + leap->push_imu(leap, timestamp_ns, sample); +} + +static inline void +xrt_tracked_leap_destroy(struct xrt_tracked_leap **xtleap_ptr) +{ + struct xrt_tracked_leap *xtleap = *xtleap_ptr; + if (xtleap == NULL) { + return; + } + + xtleap->destroy(xtleap); + *xtleap_ptr = NULL; +} + + /*! * @} */ diff --git a/src/xrt/state_trackers/gui/gui_scene_calibrate.c b/src/xrt/state_trackers/gui/gui_scene_calibrate.c index 9bfd4c12d..298b7166d 100644 --- a/src/xrt/state_trackers/gui/gui_scene_calibrate.c +++ b/src/xrt/state_trackers/gui/gui_scene_calibrate.c @@ -236,7 +236,9 @@ scene_render_select(struct gui_scene *scene, struct gui_program *p) t_calibration_stereo_create(cs->xfctx, &cs->params, &cs->status, rgb, &cali); - u_sink_create_to_yuv_or_yuyv(cs->xfctx, cali, &cali); + //u_sink_create_to_yuv_or_yuyv(cs->xfctx, cali, &cali); + u_sink_create_to_r8g8b8_or_l8(cs->xfctx, cali, &cali); + u_sink_queue_create(cs->xfctx, cali, &cali); u_sink_split_create(cs->xfctx, raw, cali, &cali); -- GitLab