initial work to make the leap motion work as a tracking camera

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()
 

src/xrt/auxiliary/tracking/t_calibration.cpp

@@ -190,6 +190,14 @@ ensure_buffers_are_allocated(class Calibration &c, int rows, int cols)
 		return;
 	}
 
+    // 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);

src/xrt/auxiliary/tracking/t_tracker_leap.cpp

+// Copyright 2019, Collabora, Ltd.
+// SPDX-License-Identifier: BSL-1.0
+/*!
+ * @file
+ * @brief  Leap Motion tracker code.
+ * @author Pete Black <pete.black@collabora.com>
+ * @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 <stdio.h>
+#include <assert.h>
+#include <pthread.h>
+
+
+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;
+}

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);
+
+
 
 /*
  *

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:

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,

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,6 +323,9 @@ v4l2_query_cap_and_validate(struct v4l2_fs *vid)
 	vid->quirks.ps4_cam =
 	    strcmp(card, "USB Camera-OV580: USB Camera-OV") == 0;
 
+    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");
@@ -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

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,
+
+
 };
 
 /*!

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

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