Commit 9d9b22a7 authored by Pete Black's avatar Pete Black

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

parent 67d33550
Pipeline #98607 failed with stages
in 2 minutes and 43 seconds
......@@ -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()
......
......@@ -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);
......
// 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;
}
......@@ -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);
/*
*
......
......@@ -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:
......
......@@ -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,
......
......@@ -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
......
......@@ -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,
};
/*!
......
......@@ -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;
}
/*!
* @}
*/
......
......@@ -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,