mirror of
https://gitlab.freedesktop.org/monado/monado.git
synced 2025-01-01 04:36:07 +00:00
d/ht: Change everything
This commit is contained in:
Moses Turner
Moses Turner
parent
8068912953
commit
624a676f56
|
|
@ -5,7 +5,6 @@
|
|||
set(ENABLED_HEADSET_DRIVERS)
|
||||
set(ENABLED_DRIVERS)
|
||||
|
||||
|
||||
if(XRT_BUILD_DRIVER_ARDUINO)
|
||||
set(ARDUINO_SOURCE_FILES
|
||||
arduino/arduino_device.c
|
||||
|
|
@ -149,18 +148,7 @@ if(XRT_BUILD_DRIVER_OHMD)
|
|||
list(APPEND ENABLED_HEADSET_DRIVERS openhmd)
|
||||
endif()
|
||||
|
||||
if(XRT_BUILD_DRIVER_HANDTRACKING)
|
||||
set(HT_SOURCE_FILES
|
||||
ht/ht_driver.c
|
||||
ht/ht_driver.h
|
||||
ht/ht_interface.h
|
||||
ht/ht_prober.c
|
||||
)
|
||||
|
||||
add_library(drv_ht STATIC ${HT_SOURCE_FILES})
|
||||
target_link_libraries(drv_ht PRIVATE xrt-interfaces aux_util aux_math)
|
||||
list(APPEND ENABLED_DRIVERS ht)
|
||||
endif()
|
||||
|
||||
if(XRT_BUILD_DRIVER_PSMV)
|
||||
set(PSMOVE_SOURCE_FILES
|
||||
|
|
@ -260,6 +248,24 @@ if(XRT_BUILD_DRIVER_VF)
|
|||
list(APPEND ENABLED_DRIVERS vf)
|
||||
endif()
|
||||
|
||||
if(XRT_BUILD_DRIVER_HANDTRACKING)
|
||||
set(HT_SOURCE_FILES
|
||||
ht/ht_driver.cpp
|
||||
ht/ht_driver.hpp
|
||||
ht/ht_interface.h
|
||||
ht/ht_models.hpp
|
||||
ht/ht_hand_math.hpp
|
||||
ht/ht_image_math.hpp
|
||||
ht/ht_nms.hpp
|
||||
ht/templates/DiscardLastBuffer.hpp
|
||||
ht/templates/NaivePermutationSort.hpp
|
||||
)
|
||||
add_library(drv_ht STATIC ${HT_SOURCE_FILES})
|
||||
target_link_libraries(drv_ht PRIVATE xrt-interfaces aux_os aux_util aux_math ONNXRuntime::ONNXRuntime ${OpenCV_LIBRARIES})
|
||||
target_include_directories(drv_ht PRIVATE ${OpenCV_INCLUDE_DIRS} ${EIGEN3_INCLUDE_DIR})
|
||||
list(APPEND ENABLED_DRIVERS ht)
|
||||
endif()
|
||||
|
||||
if (XRT_BUILD_DRIVER_SURVIVE)
|
||||
set(SURVIVE_SOURCE_FILES
|
||||
survive/survive_driver.c
|
||||
|
|
|
|||
638
src/xrt/drivers/ht/ht_algorithm.hpp
Normal file
638
src/xrt/drivers/ht/ht_algorithm.hpp
Normal file
|
|
@ -0,0 +1,638 @@
|
|||
// Copyright 2021, Collabora, Ltd.
|
||||
// SPDX-License-Identifier: BSL-1.0
|
||||
/*!
|
||||
* @file
|
||||
* @brief Camera based hand tracking mainloop algorithm.
|
||||
* @author Moses Turner <moses@collabora.com>
|
||||
* @ingroup drv_ht
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "ht_driver.hpp"
|
||||
|
||||
#include "util/u_frame.h"
|
||||
|
||||
#include "ht_image_math.hpp"
|
||||
#include "ht_models.hpp"
|
||||
#include "ht_hand_math.hpp"
|
||||
#include "templates/NaivePermutationSort.hpp"
|
||||
#include <opencv2/imgproc.hpp>
|
||||
|
||||
|
||||
// Flags to tell state tracker that these are indeed valid joints
|
||||
static enum xrt_space_relation_flags valid_flags_ht = (enum xrt_space_relation_flags)(
|
||||
XRT_SPACE_RELATION_ORIENTATION_VALID_BIT | XRT_SPACE_RELATION_ORIENTATION_TRACKED_BIT |
|
||||
XRT_SPACE_RELATION_POSITION_VALID_BIT | XRT_SPACE_RELATION_POSITION_TRACKED_BIT);
|
||||
|
||||
|
||||
static void
|
||||
htProcessJoint(struct ht_device *htd,
|
||||
struct xrt_vec3 model_out,
|
||||
struct xrt_hand_joint_set *hand,
|
||||
enum xrt_hand_joint idx)
|
||||
{
|
||||
hand->values.hand_joint_set_default[idx].relation.relation_flags = valid_flags_ht;
|
||||
hand->values.hand_joint_set_default[idx].relation.pose.position.x = model_out.x;
|
||||
hand->values.hand_joint_set_default[idx].relation.pose.position.y = model_out.y;
|
||||
hand->values.hand_joint_set_default[idx].relation.pose.position.z = model_out.z;
|
||||
}
|
||||
|
||||
static float
|
||||
errHistory2D(HandHistory2DBBox *past, Palm7KP *present)
|
||||
{
|
||||
return (m_vec2_len(*past->wrist_unfiltered[0] - present->kps[WRIST_7KP]) +
|
||||
m_vec2_len(*past->middle_unfiltered[0] - present->kps[MIDDLE_7KP]));
|
||||
}
|
||||
|
||||
static std::vector<Hand2D>
|
||||
htImageToKeypoints(struct ht_view *htv)
|
||||
{
|
||||
int view = htv->view;
|
||||
struct ht_device *htd = htv->htd;
|
||||
|
||||
|
||||
cv::Mat raw_input = htv->run_model_on_this;
|
||||
|
||||
// Get a list of palms - drop confidences and ssd bounding boxes, just keypoints.
|
||||
|
||||
std::vector<Palm7KP> hand_detections = htv->run_detection_model(htv, raw_input);
|
||||
|
||||
std::vector<bool> used_histories;
|
||||
std::vector<bool> used_detections;
|
||||
|
||||
std::vector<size_t> history_indices;
|
||||
std::vector<size_t> detection_indices;
|
||||
std::vector<float> dontuse;
|
||||
|
||||
|
||||
// Strategy here is: We have a big list of palms. Match 'em up to previous palms.
|
||||
naive_sort_permutation_by_error<HandHistory2DBBox, Palm7KP>(htv->bbox_histories, hand_detections,
|
||||
|
||||
// bools
|
||||
used_histories, used_detections,
|
||||
|
||||
history_indices, detection_indices, dontuse,
|
||||
errHistory2D);
|
||||
|
||||
// Here's the trick - we use the associated bbox_filter to get an output but *never commit* the noisy 128x128
|
||||
// detection; instead later on we commit the (hopefully) nicer palm and wrist from the 224x224 keypoint
|
||||
// estimation.
|
||||
|
||||
// Add extra detections!
|
||||
for (size_t i = 0; i < used_detections.size(); i++) {
|
||||
if (used_detections[i] == false) {
|
||||
HandHistory2DBBox hist_new = {};
|
||||
m_filter_euro_vec2_init(&hist_new.m_filter_middle, FCMIN_BBOX, FCMIN_D_BB0X, BETA_BB0X);
|
||||
m_filter_euro_vec2_init(&hist_new.m_filter_wrist, FCMIN_BBOX, FCMIN_D_BB0X, BETA_BB0X);
|
||||
|
||||
// this leaks, on august 24
|
||||
htv->bbox_histories.push_back(hist_new);
|
||||
history_indices.push_back(htv->bbox_histories.size() - 1);
|
||||
detection_indices.push_back(i);
|
||||
}
|
||||
}
|
||||
|
||||
// Do the things for each active bbox history!
|
||||
for (size_t i = 0; i < history_indices.size(); i++) {
|
||||
HandHistory2DBBox *hist_of_interest = &htv->bbox_histories[history_indices[i]];
|
||||
hist_of_interest->wrist_unfiltered.push(hand_detections[detection_indices[i]].kps[WRIST_7KP]);
|
||||
hist_of_interest->middle_unfiltered.push(hand_detections[detection_indices[i]].kps[MIDDLE_7KP]);
|
||||
// Eh do the rest later
|
||||
}
|
||||
|
||||
// Prune stale detections! (After we don't need {history,detection}_indices to be correct)
|
||||
int bob = 0;
|
||||
for (size_t i = 0; i < used_histories.size(); i++) {
|
||||
if (used_histories[i] == false) {
|
||||
// history never got assigned a present hand to it. treat it as stale delete it.
|
||||
|
||||
HT_TRACE(htv->htd, "Removing bbox from history!\n");
|
||||
htv->bbox_histories.erase(htv->bbox_histories.begin() + i + bob);
|
||||
bob--;
|
||||
}
|
||||
}
|
||||
if (htv->bbox_histories.size() == 0) {
|
||||
return {}; // bail early
|
||||
}
|
||||
|
||||
|
||||
|
||||
std::vector<Hand2D> list_of_hands_in_bbox(
|
||||
htv->bbox_histories.size()); // all of these are same size as htv->bbox_histories
|
||||
|
||||
std::vector<std::future<Hand2D>> await_list_of_hand_in_bbox; //(htv->bbox_histories.size());
|
||||
|
||||
std::vector<DetectionModelOutput> blah(htv->bbox_histories.size());
|
||||
|
||||
std::vector<Hand2D> output;
|
||||
|
||||
if (htv->bbox_histories.size() > 2) {
|
||||
HT_DEBUG(htd, "More than two hands (%zu) in 2D view %i", htv->bbox_histories.size(), htv->view);
|
||||
}
|
||||
|
||||
|
||||
for (size_t i = 0; i < htv->bbox_histories.size(); i++) { //(BBoxHistory * entry : htv->bbox_histories) {
|
||||
HandHistory2DBBox *entry = &htv->bbox_histories[i];
|
||||
cv::Mat hand_rect = cv::Mat(224, 224, CV_8UC3);
|
||||
xrt_vec2 goodenough_middle;
|
||||
xrt_vec2 goodenough_wrist;
|
||||
|
||||
m_filter_euro_vec2_run_no_commit(&entry->m_filter_middle, htv->htd->current_frame_timestamp,
|
||||
entry->middle_unfiltered[0], &goodenough_middle);
|
||||
m_filter_euro_vec2_run_no_commit(&entry->m_filter_wrist, htv->htd->current_frame_timestamp,
|
||||
entry->wrist_unfiltered[0], &goodenough_wrist);
|
||||
|
||||
rotatedRectFromJoints(htv, goodenough_middle, goodenough_wrist, &blah[i]);
|
||||
|
||||
|
||||
|
||||
warpAffine(raw_input, hand_rect, blah[i].warp_there, hand_rect.size());
|
||||
|
||||
await_list_of_hand_in_bbox.push_back(
|
||||
std::async(std::launch::async, htd->views[view].run_keypoint_model, &htd->views[view], hand_rect));
|
||||
}
|
||||
|
||||
// cut here
|
||||
|
||||
for (size_t i = 0; i < htv->bbox_histories.size(); i++) {
|
||||
|
||||
Hand2D in_bbox = await_list_of_hand_in_bbox[i].get();
|
||||
|
||||
cv::Matx23f warp_back = blah[i].warp_back;
|
||||
|
||||
Hand2D in_image_ray_coords;
|
||||
Hand2D in_image_px_coords;
|
||||
|
||||
for (int i = 0; i < 21; i++) {
|
||||
struct xrt_vec3 vec = in_bbox.kps[i];
|
||||
|
||||
#if 1
|
||||
xrt_vec3 rr = transformVecBy2x3(vec, warp_back);
|
||||
rr.z = vec.z;
|
||||
#else
|
||||
xrt_vec3 rr;
|
||||
rr.x = (vec.x * warp_back(0, 0)) + (vec.y * warp_back(0, 1)) + warp_back(0, 2);
|
||||
rr.y = (vec.x * warp_back(1, 0)) + (vec.y * warp_back(1, 1)) + warp_back(1, 2);
|
||||
rr.z = vec.z;
|
||||
#endif
|
||||
in_image_px_coords.kps[i] = rr;
|
||||
|
||||
in_image_ray_coords.kps[i] = raycoord(htv, rr);
|
||||
if (htd->debug_scribble) {
|
||||
handDot(htv->debug_out_to_this, {rr.x, rr.y}, fmax((-vec.z + 100 - 20) * .08, 2),
|
||||
((float)i) / 21.0f, cv::FILLED);
|
||||
}
|
||||
}
|
||||
xrt_vec2 middle_in_px_coords = {in_image_px_coords.kps[MIDL_PXM].x, in_image_px_coords.kps[MIDL_PXM].y};
|
||||
xrt_vec2 wrist_in_px_coords = {in_image_px_coords.kps[WRIST].x, in_image_px_coords.kps[WRIST].y};
|
||||
xrt_vec2 dontuse;
|
||||
m_filter_euro_vec2_run(&htv->bbox_histories[i].m_filter_wrist, htv->htd->current_frame_timestamp,
|
||||
&wrist_in_px_coords, &dontuse);
|
||||
|
||||
m_filter_euro_vec2_run(&htv->bbox_histories[i].m_filter_middle, htv->htd->current_frame_timestamp,
|
||||
&middle_in_px_coords, &dontuse);
|
||||
output.push_back(in_image_ray_coords);
|
||||
}
|
||||
return output;
|
||||
}
|
||||
|
||||
#if defined(JSON_OUTPUT)
|
||||
|
||||
static void
|
||||
jsonAddJoint(cJSON *into_this, xrt_pose loc, const char *name)
|
||||
{
|
||||
cJSON *container = cJSON_CreateObject();
|
||||
cJSON *joint_loc = cJSON_CreateArray();
|
||||
cJSON_AddItemToArray(joint_loc, cJSON_CreateNumber(loc.position.x));
|
||||
cJSON_AddItemToArray(joint_loc, cJSON_CreateNumber(loc.position.y));
|
||||
cJSON_AddItemToArray(joint_loc, cJSON_CreateNumber(loc.position.z));
|
||||
|
||||
cJSON_AddItemToObject(container, "position", joint_loc);
|
||||
|
||||
cJSON *joint_rot = cJSON_CreateArray();
|
||||
|
||||
|
||||
cJSON_AddItemToArray(joint_rot, cJSON_CreateNumber(loc.orientation.x));
|
||||
cJSON_AddItemToArray(joint_rot, cJSON_CreateNumber(loc.orientation.y));
|
||||
cJSON_AddItemToArray(joint_rot, cJSON_CreateNumber(loc.orientation.z));
|
||||
cJSON_AddItemToArray(joint_rot, cJSON_CreateNumber(loc.orientation.w));
|
||||
|
||||
cJSON_AddItemToObject(container, "rotation_quat_xyzw", joint_rot);
|
||||
|
||||
cJSON_AddItemToObject(into_this, name, container);
|
||||
}
|
||||
|
||||
|
||||
static void
|
||||
jsonAddSet(struct ht_device *htd)
|
||||
{
|
||||
cJSON *two_hand_container = cJSON_CreateObject();
|
||||
static const char *keys[] = {
|
||||
"wrist", "palm",
|
||||
|
||||
"thumb_mcp", "thumb_pxm", "thumb_dst", "thumb_tip",
|
||||
|
||||
"index_mcp", "index_pxm", "index_int", "index_dst", "index_tip",
|
||||
|
||||
"middle_mcp", "middle_pxm", "middle_int", "middle_dst", "middle_tip",
|
||||
|
||||
"ring_mcp", "ring_pxm", "ring_int", "ring_dst", "ring_tip",
|
||||
|
||||
"little_mcp", "little_pxm", "little_int", "little_dst", "little_tip",
|
||||
};
|
||||
static const char *sides_names[] = {
|
||||
"left",
|
||||
"right",
|
||||
};
|
||||
for (int side = 0; side < 2; side++) {
|
||||
struct xrt_hand_joint_set *set = &htd->hands_for_openxr[side];
|
||||
if (!set->is_active) {
|
||||
cJSON_AddNullToObject(two_hand_container, sides_names[side]);
|
||||
} else {
|
||||
cJSON *hand_obj = cJSON_CreateObject();
|
||||
for (int i = 0; i < 26; i++) {
|
||||
const char *key = keys[i];
|
||||
xrt_pose pose = set->values.hand_joint_set_default[i].relation.pose;
|
||||
jsonAddJoint(hand_obj, pose, key);
|
||||
}
|
||||
cJSON_AddItemToObject(two_hand_container, sides_names[side], hand_obj);
|
||||
}
|
||||
}
|
||||
|
||||
#if defined(JSON_OUTPUT)
|
||||
cJSON_AddItemToArray(htd->output_array, two_hand_container);
|
||||
#endif
|
||||
}
|
||||
|
||||
#endif
|
||||
|
||||
|
||||
static void
|
||||
htBailThisFrame(struct ht_device *htd)
|
||||
{
|
||||
|
||||
os_mutex_lock(&htd->openxr_hand_data_mediator);
|
||||
htd->hands_for_openxr[0].is_active = false;
|
||||
htd->hands_for_openxr[1].is_active = false;
|
||||
#if defined(JSON_OUTPUT)
|
||||
json_add_set(htd);
|
||||
#endif
|
||||
os_mutex_unlock(&htd->openxr_hand_data_mediator);
|
||||
}
|
||||
|
||||
int64_t last_frame, this_frame;
|
||||
|
||||
static void
|
||||
htRunAlgorithm(struct ht_device *htd)
|
||||
{
|
||||
XRT_TRACE_MARKER();
|
||||
|
||||
htd->current_frame_timestamp = htd->frame_for_process->timestamp;
|
||||
|
||||
|
||||
int64_t start, end;
|
||||
start = os_monotonic_get_ns();
|
||||
|
||||
|
||||
|
||||
cv::Mat full_frame(960, 960 * 2, CV_8UC3, htd->frame_for_process->data, htd->frame_for_process->stride);
|
||||
htd->views[0].run_model_on_this =
|
||||
full_frame(cv::Rect(0, 0, htd->camera.one_view_size_px.w, htd->camera.one_view_size_px.h));
|
||||
htd->views[1].run_model_on_this = full_frame(cv::Rect(
|
||||
htd->camera.one_view_size_px.w, 0, htd->camera.one_view_size_px.w, htd->camera.one_view_size_px.h));
|
||||
|
||||
// Check this every frame. We really, really, really don't want it to ever suddenly be null.
|
||||
htd->debug_scribble = htd->debug_sink != nullptr;
|
||||
|
||||
cv::Mat debug_output;
|
||||
xrt_frame *debug_frame = nullptr; // only use if htd->debug_scribble
|
||||
|
||||
|
||||
if (htd->debug_scribble) {
|
||||
u_frame_clone(htd->frame_for_process, &debug_frame);
|
||||
debug_output = cv::Mat(960, 960 * 2, CV_8UC3, debug_frame->data, debug_frame->stride);
|
||||
htd->views[0].debug_out_to_this =
|
||||
debug_output(cv::Rect(0, 0, htd->camera.one_view_size_px.w, htd->camera.one_view_size_px.h));
|
||||
htd->views[1].debug_out_to_this = debug_output(cv::Rect(
|
||||
htd->camera.one_view_size_px.w, 0, htd->camera.one_view_size_px.w, htd->camera.one_view_size_px.h));
|
||||
}
|
||||
|
||||
std::future<std::vector<Hand2D>> future_left =
|
||||
std::async(std::launch::async, htImageToKeypoints, &htd->views[0]);
|
||||
std::future<std::vector<Hand2D>> future_right =
|
||||
std::async(std::launch::async, htImageToKeypoints, &htd->views[1]);
|
||||
std::vector<Hand2D> hands_in_left_view = future_left.get();
|
||||
std::vector<Hand2D> hands_in_right_view = future_right.get();
|
||||
end = os_monotonic_get_ns();
|
||||
|
||||
|
||||
this_frame = os_monotonic_get_ns();
|
||||
|
||||
double time_ms = (double)(end - start) / (double)U_TIME_1MS_IN_NS;
|
||||
double _1_time = 1 / (time_ms * 0.001);
|
||||
|
||||
char t[64];
|
||||
char t2[64];
|
||||
sprintf(t, "% 8.2f ms", time_ms);
|
||||
sprintf(t2, "% 8.2f fps", _1_time);
|
||||
last_frame = this_frame;
|
||||
|
||||
|
||||
if (htd->debug_scribble) {
|
||||
cv::putText(debug_output, t, cv::Point(30, 60), cv::FONT_HERSHEY_SIMPLEX, 1.0f, cv::Scalar(0, 255, 0),
|
||||
4);
|
||||
cv::putText(debug_output, t2, cv::Point(30, 100), cv::FONT_HERSHEY_SIMPLEX, 1.0f, cv::Scalar(0, 255, 0),
|
||||
4);
|
||||
} else {
|
||||
HT_DEBUG(htd, "%s", t);
|
||||
HT_DEBUG(htd, "%s", t2);
|
||||
}
|
||||
|
||||
|
||||
// Convenience
|
||||
uint64_t timestamp = htd->frame_for_process->timestamp;
|
||||
|
||||
if (htd->debug_scribble) {
|
||||
htd->debug_sink->push_frame(htd->debug_sink, debug_frame);
|
||||
xrt_frame_reference(&debug_frame, NULL);
|
||||
}
|
||||
|
||||
// Bail early this frame if no hands were detected.
|
||||
// In the long run, this'll be a silly thing - we shouldn't always take the detection model's word for it
|
||||
// especially when part of the pipeline is an arbitrary confidence threshold.
|
||||
if (hands_in_left_view.size() == 0 || hands_in_right_view.size() == 0) {
|
||||
htBailThisFrame(htd);
|
||||
return;
|
||||
}
|
||||
|
||||
// Figure out how to match hands up across views.
|
||||
// Construct a matrix, where the rows are left view hands and the cols are right view hands.
|
||||
// For each cell, compute an error that's just the difference in Y ray coordinates of all the 21 keypoints. With
|
||||
// perfect cameras + models, these differences will be zero. Anything with a high difference is not the same
|
||||
// hand observed across views. For each cell, make a datatype that is: the error, the left view hand index, the
|
||||
// right view hand index. Put these in an array, sort them by lowest error. Iterate over this sorted list (not
|
||||
// in matrix-land anymore), assigning left view hands to right view hands as you go. For any elements that are
|
||||
// trying to assign an already-assigned hand, skip them. At the end, check for any hands that went un-assigned;
|
||||
// forget about those.
|
||||
|
||||
// In the future, maybe we should go forward with several hand associations if there are two that are close,
|
||||
// keep track of which associations are mutually exclusive, and drop the one that fits the kinematic model less
|
||||
// well? Or drop the one that matches with previous measurements less well? Getting raw 3D poses out of line
|
||||
// intersection is not expensive.
|
||||
|
||||
// Known issue: If you put your hands at both exactly the same height it will not do the right thing. Won't fix
|
||||
// right now; need to upstream *something* first.
|
||||
|
||||
std::vector<bool> left_hands_taken;
|
||||
std::vector<bool> right_hands_taken;
|
||||
|
||||
std::vector<size_t> l_indices_in_order;
|
||||
std::vector<size_t> r_indices_in_order;
|
||||
std::vector<float> y_disparity_error_in_order;
|
||||
|
||||
naive_sort_permutation_by_error<Hand2D, Hand2D>(
|
||||
// Inputs
|
||||
hands_in_left_view, hands_in_right_view,
|
||||
|
||||
// Outputs
|
||||
left_hands_taken, right_hands_taken,
|
||||
|
||||
l_indices_in_order, r_indices_in_order, y_disparity_error_in_order, errHandDisparity);
|
||||
|
||||
std::vector<Hand2D> associated_in_left;
|
||||
std::vector<Hand2D> associated_in_right;
|
||||
|
||||
|
||||
for (size_t i = 0; i < l_indices_in_order.size(); i++) {
|
||||
associated_in_left.push_back(hands_in_left_view[i]);
|
||||
associated_in_right.push_back(hands_in_right_view[i]);
|
||||
}
|
||||
|
||||
|
||||
std::vector<Hand3D> hands_unfiltered; //(associated_in_left.size());
|
||||
|
||||
for (size_t hand_idx = 0; hand_idx < associated_in_left.size(); hand_idx++) {
|
||||
|
||||
Hand3D cur_hand;
|
||||
|
||||
for (int i = 0; i < 21; i++) {
|
||||
float t = htd->baseline /
|
||||
(associated_in_left[hand_idx].kps[i].x - associated_in_right[hand_idx].kps[i].x);
|
||||
// float x, y;
|
||||
|
||||
cur_hand.kps[i].z = -t;
|
||||
|
||||
cur_hand.kps[i].x = (associated_in_left[hand_idx].kps[i].x * t); //-(htd->baseline * 0.5f);
|
||||
cur_hand.kps[i].y = -associated_in_left[hand_idx].kps[i].y * t;
|
||||
cur_hand.timestamp = timestamp;
|
||||
|
||||
// soon! average with hand in right view.
|
||||
cur_hand.kps[i].x += htd->baseline + (associated_in_right[hand_idx].kps[i].x * t);
|
||||
cur_hand.kps[i].y += -associated_in_right[hand_idx].kps[i].y * t;
|
||||
|
||||
cur_hand.kps[i].x *= .5;
|
||||
cur_hand.kps[i].y *= .5;
|
||||
}
|
||||
|
||||
if (rejectBadHand(&cur_hand)) { // reject hands!!!
|
||||
cur_hand.y_disparity_error = y_disparity_error_in_order[hand_idx];
|
||||
hands_unfiltered.push_back(cur_hand);
|
||||
} else {
|
||||
HT_DEBUG(htd, "Rejected bad hand!"); // This probably could be a warn ...
|
||||
}
|
||||
}
|
||||
|
||||
// Okay now do the exact same thing but with present and past instead of with left view and right view. Lotsa
|
||||
// code but hey this is hard stuff.
|
||||
|
||||
|
||||
std::vector<bool> past_hands_taken;
|
||||
std::vector<bool> present_hands_taken;
|
||||
|
||||
std::vector<size_t> past_indices;
|
||||
std::vector<size_t> present_indices;
|
||||
std::vector<float> flow_errors;
|
||||
|
||||
|
||||
naive_sort_permutation_by_error<HandHistory3D, Hand3D>(htd->histories_3d, // past
|
||||
hands_unfiltered, // present
|
||||
|
||||
|
||||
// outputs
|
||||
past_hands_taken, present_hands_taken, past_indices,
|
||||
present_indices, flow_errors, errHandHistory
|
||||
|
||||
);
|
||||
|
||||
|
||||
for (size_t i = 0; i < past_indices.size(); i++) {
|
||||
htd->histories_3d[past_indices[i]].last_hands.push(hands_unfiltered[present_indices[i]]);
|
||||
}
|
||||
// The above may not do anything, because we'll start out with no hand histories! All the numbers of elements
|
||||
// should be zero.
|
||||
|
||||
|
||||
for (size_t i = 0; i < present_hands_taken.size(); i++) {
|
||||
if (present_hands_taken[i] == false) {
|
||||
// if this hand never got assigned to a history
|
||||
HandHistory3D history_new;
|
||||
handEuroFiltersInit(&history_new, FCMIN_HAND, FCMIN_D_HAND, BETA_HAND);
|
||||
history_new.last_hands.push(hands_unfiltered[i]);
|
||||
// history_new.
|
||||
htd->histories_3d.push_back(
|
||||
history_new); // Add something to the end - don't initialize any of it.
|
||||
}
|
||||
}
|
||||
|
||||
int bob = 0;
|
||||
for (size_t i = 0; i < past_hands_taken.size(); i++) {
|
||||
if (past_hands_taken[i] == false) {
|
||||
htd->histories_3d.erase(htd->histories_3d.begin() + i + bob);
|
||||
bob--;
|
||||
}
|
||||
}
|
||||
|
||||
if (htd->histories_3d.size() == 0) {
|
||||
HT_DEBUG(htd, "Bailing");
|
||||
htBailThisFrame(htd);
|
||||
return;
|
||||
}
|
||||
|
||||
size_t num_hands = htd->histories_3d.size();
|
||||
if (num_hands > 2) {
|
||||
HT_WARN(htd, "More than two hands observed (%zu)! Expect bugginess!",
|
||||
num_hands); // this is quite bad, but rarely happens.
|
||||
}
|
||||
|
||||
// Iterate over all hands we're keeping track of, compute their current handedness.
|
||||
for (size_t i = 0; i < htd->histories_3d.size(); i++) {
|
||||
handednessHandHistory3D(&htd->histories_3d[i]);
|
||||
}
|
||||
|
||||
// Whoo! Okay, now we have some unfiltered hands in htd->histories_3d[i].last_hands[0]! Euro filter them!
|
||||
|
||||
std::vector<Hand3D> filtered_hands(num_hands);
|
||||
|
||||
for (size_t hand_index = 0; hand_index < num_hands; hand_index++) {
|
||||
filtered_hands[hand_index] = handEuroFiltersRun(&htd->histories_3d[hand_index]);
|
||||
applyThumbIndexDrag(&filtered_hands[hand_index]);
|
||||
filtered_hands[hand_index].handedness = htd->histories_3d[hand_index].handedness;
|
||||
}
|
||||
|
||||
std::vector<size_t> xr_indices;
|
||||
std::vector<Hand3D *> hands_to_use;
|
||||
|
||||
if (filtered_hands.size() == 1) {
|
||||
if (filtered_hands[0].handedness < 0) {
|
||||
// Left
|
||||
xr_indices = {0};
|
||||
hands_to_use = {&filtered_hands[0]};
|
||||
} else {
|
||||
xr_indices = {1};
|
||||
hands_to_use = {&filtered_hands[0]};
|
||||
}
|
||||
} else {
|
||||
// filtered_hands better be two for now.
|
||||
if (filtered_hands[0].handedness < filtered_hands[1].handedness) {
|
||||
xr_indices = {0, 1};
|
||||
hands_to_use = {&filtered_hands[0], &filtered_hands[1]};
|
||||
} else {
|
||||
xr_indices = {1, 0};
|
||||
hands_to_use = {&filtered_hands[0], &filtered_hands[1]};
|
||||
}
|
||||
}
|
||||
|
||||
struct xrt_hand_joint_set final_hands_ordered_by_handedness[2];
|
||||
memset(&final_hands_ordered_by_handedness[0], 0, sizeof(xrt_hand_joint_set));
|
||||
memset(&final_hands_ordered_by_handedness[1], 0, sizeof(xrt_hand_joint_set));
|
||||
final_hands_ordered_by_handedness[0].is_active = false;
|
||||
final_hands_ordered_by_handedness[1].is_active = false;
|
||||
|
||||
for (size_t i = 0; (i < xr_indices.size()); i++) {
|
||||
Hand3D *hand = hands_to_use[i];
|
||||
|
||||
|
||||
struct xrt_hand_joint_set *put_in_set = &final_hands_ordered_by_handedness[xr_indices[i]];
|
||||
|
||||
xrt_vec3 wrist = hand->kps[0];
|
||||
|
||||
xrt_vec3 index_prox = hand->kps[5];
|
||||
xrt_vec3 middle_prox = hand->kps[9];
|
||||
xrt_vec3 ring_prox = hand->kps[13];
|
||||
xrt_vec3 pinky_prox = hand->kps[17];
|
||||
|
||||
xrt_vec3 middle_to_index = m_vec3_sub(index_prox, middle_prox);
|
||||
xrt_vec3 middle_to_ring = m_vec3_sub(ring_prox, middle_prox);
|
||||
xrt_vec3 middle_to_pinky = m_vec3_sub(pinky_prox, middle_prox);
|
||||
|
||||
xrt_vec3 three_fourths_down_middle_mcp =
|
||||
m_vec3_add(m_vec3_mul_scalar(wrist, 3.0f / 4.0f), m_vec3_mul_scalar(middle_prox, 1.0f / 4.0f));
|
||||
|
||||
xrt_vec3 middle_metacarpal = three_fourths_down_middle_mcp;
|
||||
|
||||
float s = 0.6f;
|
||||
|
||||
xrt_vec3 index_metacarpal = middle_metacarpal + m_vec3_mul_scalar(middle_to_index, s);
|
||||
xrt_vec3 ring_metacarpal = middle_metacarpal + m_vec3_mul_scalar(middle_to_ring, s);
|
||||
xrt_vec3 pinky_metacarpal = middle_metacarpal + m_vec3_mul_scalar(middle_to_pinky, s);
|
||||
|
||||
float palm_ness = 0.33;
|
||||
xrt_vec3 palm =
|
||||
m_vec3_add(m_vec3_mul_scalar(wrist, palm_ness), m_vec3_mul_scalar(middle_prox, (1.0f - palm_ness)));
|
||||
|
||||
|
||||
|
||||
// clang-format off
|
||||
|
||||
htProcessJoint(htd,palm, put_in_set, XRT_HAND_JOINT_PALM);
|
||||
|
||||
htProcessJoint(htd,hand->kps[0], put_in_set, XRT_HAND_JOINT_WRIST);
|
||||
htProcessJoint(htd,hand->kps[1], put_in_set, XRT_HAND_JOINT_THUMB_METACARPAL);
|
||||
htProcessJoint(htd,hand->kps[2], put_in_set, XRT_HAND_JOINT_THUMB_PROXIMAL);
|
||||
htProcessJoint(htd,hand->kps[3], put_in_set, XRT_HAND_JOINT_THUMB_DISTAL);
|
||||
htProcessJoint(htd,hand->kps[4], put_in_set, XRT_HAND_JOINT_THUMB_TIP);
|
||||
|
||||
htProcessJoint(htd,index_metacarpal, put_in_set, XRT_HAND_JOINT_INDEX_METACARPAL);
|
||||
htProcessJoint(htd,hand->kps[5], put_in_set, XRT_HAND_JOINT_INDEX_PROXIMAL);
|
||||
htProcessJoint(htd,hand->kps[6], put_in_set, XRT_HAND_JOINT_INDEX_INTERMEDIATE);
|
||||
htProcessJoint(htd,hand->kps[7], put_in_set, XRT_HAND_JOINT_INDEX_DISTAL);
|
||||
htProcessJoint(htd,hand->kps[8], put_in_set, XRT_HAND_JOINT_INDEX_TIP);
|
||||
|
||||
htProcessJoint(htd,middle_metacarpal, put_in_set, XRT_HAND_JOINT_MIDDLE_METACARPAL);
|
||||
htProcessJoint(htd,hand->kps[9], put_in_set, XRT_HAND_JOINT_MIDDLE_PROXIMAL);
|
||||
htProcessJoint(htd,hand->kps[10], put_in_set, XRT_HAND_JOINT_MIDDLE_INTERMEDIATE);
|
||||
htProcessJoint(htd,hand->kps[11], put_in_set, XRT_HAND_JOINT_MIDDLE_DISTAL);
|
||||
htProcessJoint(htd,hand->kps[12], put_in_set, XRT_HAND_JOINT_MIDDLE_TIP);
|
||||
|
||||
htProcessJoint(htd,ring_metacarpal, put_in_set, XRT_HAND_JOINT_RING_METACARPAL);
|
||||
htProcessJoint(htd,hand->kps[13], put_in_set, XRT_HAND_JOINT_RING_PROXIMAL);
|
||||
htProcessJoint(htd,hand->kps[14], put_in_set, XRT_HAND_JOINT_RING_INTERMEDIATE);
|
||||
htProcessJoint(htd,hand->kps[15], put_in_set, XRT_HAND_JOINT_RING_DISTAL);
|
||||
htProcessJoint(htd,hand->kps[16], put_in_set, XRT_HAND_JOINT_RING_TIP);
|
||||
|
||||
htProcessJoint(htd, pinky_metacarpal, put_in_set, XRT_HAND_JOINT_LITTLE_METACARPAL);
|
||||
htProcessJoint(htd,hand->kps[17], put_in_set, XRT_HAND_JOINT_LITTLE_PROXIMAL);
|
||||
htProcessJoint(htd,hand->kps[18], put_in_set, XRT_HAND_JOINT_LITTLE_INTERMEDIATE);
|
||||
htProcessJoint(htd,hand->kps[19], put_in_set, XRT_HAND_JOINT_LITTLE_DISTAL);
|
||||
htProcessJoint(htd,hand->kps[20], put_in_set, XRT_HAND_JOINT_LITTLE_TIP);
|
||||
put_in_set->is_active = true;
|
||||
math_pose_identity(&put_in_set->hand_pose.pose);
|
||||
put_in_set->hand_pose.relation_flags = valid_flags_ht;
|
||||
// clang-format on
|
||||
applyJointWidths(put_in_set);
|
||||
applyJointOrientations(put_in_set, xr_indices[i]);
|
||||
}
|
||||
|
||||
|
||||
// For some reason, final_hands_ordered_by_handedness[0] is active but the other is inactive.
|
||||
|
||||
os_mutex_lock(&htd->openxr_hand_data_mediator);
|
||||
memcpy(&htd->hands_for_openxr[0], &final_hands_ordered_by_handedness[0], sizeof(struct xrt_hand_joint_set));
|
||||
memcpy(&htd->hands_for_openxr[1], &final_hands_ordered_by_handedness[1], sizeof(struct xrt_hand_joint_set));
|
||||
|
||||
#if defined(JSON_OUTPUT)
|
||||
json_add_set(htd);
|
||||
#endif
|
||||
os_mutex_unlock(&htd->openxr_hand_data_mediator);
|
||||
}
|
||||
|
|
@ -1,146 +0,0 @@
|
|||
// Copyright 2020, Collabora, Ltd.
|
||||
// SPDX-License-Identifier: BSL-1.0
|
||||
/*!
|
||||
* @file
|
||||
* @brief Camera based hand tracking driver code.
|
||||
* @author Christtoph Haag <christtoph.haag@collabora.com>
|
||||
* @ingroup drv_ht
|
||||
*/
|
||||
|
||||
#include "ht_driver.h"
|
||||
#include "util/u_device.h"
|
||||
#include "util/u_var.h"
|
||||
#include "util/u_debug.h"
|
||||
#include <string.h>
|
||||
#include <stdio.h>
|
||||
|
||||
struct ht_device
|
||||
{
|
||||
struct xrt_device base;
|
||||
|
||||
struct xrt_tracked_hand *tracker;
|
||||
|
||||
struct xrt_space_relation hand_relation[2];
|
||||
struct u_hand_tracking u_tracking[2];
|
||||
|
||||
struct xrt_tracking_origin tracking_origin;
|
||||
|
||||
enum u_logging_level ll;
|
||||
};
|
||||
|
||||
DEBUG_GET_ONCE_LOG_OPTION(ht_log, "HT_LOG", U_LOGGING_WARN)
|
||||
|
||||
#define HT_TRACE(htd, ...) U_LOG_XDEV_IFL_T(&htd->base, htd->ll, __VA_ARGS__)
|
||||
#define HT_DEBUG(htd, ...) U_LOG_XDEV_IFL_D(&htd->base, htd->ll, __VA_ARGS__)
|
||||
#define HT_INFO(htd, ...) U_LOG_XDEV_IFL_I(&htd->base, htd->ll, __VA_ARGS__)
|
||||
#define HT_WARN(htd, ...) U_LOG_XDEV_IFL_W(&htd->base, htd->ll, __VA_ARGS__)
|
||||
#define HT_ERROR(htd, ...) U_LOG_XDEV_IFL_E(&htd->base, htd->ll, __VA_ARGS__)
|
||||
|
||||
static inline struct ht_device *
|
||||
ht_device(struct xrt_device *xdev)
|
||||
{
|
||||
return (struct ht_device *)xdev;
|
||||
}
|
||||
|
||||
static void
|
||||
ht_device_update_inputs(struct xrt_device *xdev)
|
||||
{
|
||||
// Empty
|
||||
}
|
||||
|
||||
static void
|
||||
ht_device_get_hand_tracking(struct xrt_device *xdev,
|
||||
enum xrt_input_name name,
|
||||
uint64_t at_timestamp_ns,
|
||||
struct xrt_hand_joint_set *out_value)
|
||||
{
|
||||
struct ht_device *htd = ht_device(xdev);
|
||||
|
||||
enum xrt_hand hand;
|
||||
int index;
|
||||
|
||||
if (name == XRT_INPUT_GENERIC_HAND_TRACKING_LEFT) {
|
||||
HT_TRACE(htd, "Get left hand tracking data");
|
||||
index = 0;
|
||||
hand = XRT_HAND_LEFT;
|
||||
} else if (name == XRT_INPUT_GENERIC_HAND_TRACKING_RIGHT) {
|
||||
HT_TRACE(htd, "Get right hand tracking data");
|
||||
index = 1;
|
||||
hand = XRT_HAND_RIGHT;
|
||||
} else {
|
||||
HT_ERROR(htd, "unknown input name for hand tracker");
|
||||
return;
|
||||
}
|
||||
|
||||
|
||||
|
||||
htd->tracker->get_tracked_joints(htd->tracker, name, at_timestamp_ns, &htd->u_tracking[index].joints,
|
||||
&htd->hand_relation[index]);
|
||||
htd->u_tracking[index].timestamp_ns = at_timestamp_ns;
|
||||
|
||||
struct xrt_pose identity = XRT_POSE_IDENTITY;
|
||||
|
||||
u_hand_joints_set_out_data(&htd->u_tracking[index], hand, &htd->hand_relation[index], &identity, out_value);
|
||||
}
|
||||
|
||||
static void
|
||||
ht_device_destroy(struct xrt_device *xdev)
|
||||
{
|
||||
struct ht_device *htd = ht_device(xdev);
|
||||
|
||||
// Remove the variable tracking.
|
||||
u_var_remove_root(htd);
|
||||
|
||||
u_device_free(&htd->base);
|
||||
}
|
||||
|
||||
struct xrt_device *
|
||||
ht_device_create(struct xrt_auto_prober *xap, cJSON *attached_data, struct xrt_prober *xp)
|
||||
{
|
||||
enum u_device_alloc_flags flags = U_DEVICE_ALLOC_NO_FLAGS;
|
||||
|
||||
//! @todo 2 hands hardcoded
|
||||
int num_hands = 2;
|
||||
|
||||
struct ht_device *htd = U_DEVICE_ALLOCATE(struct ht_device, flags, num_hands, 0);
|
||||
|
||||
|
||||
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->ll = debug_get_log_option_ht_log();
|
||||
|
||||
htd->base.update_inputs = ht_device_update_inputs;
|
||||
htd->base.get_hand_tracking = ht_device_get_hand_tracking;
|
||||
htd->base.destroy = ht_device_destroy;
|
||||
|
||||
snprintf(htd->base.str, XRT_DEVICE_NAME_LEN, "Camera based Hand Tracker");
|
||||
snprintf(htd->base.serial, XRT_DEVICE_NAME_LEN, "Camera based Hand Tracker");
|
||||
|
||||
htd->base.inputs[0].name = XRT_INPUT_GENERIC_HAND_TRACKING_LEFT;
|
||||
htd->base.inputs[1].name = XRT_INPUT_GENERIC_HAND_TRACKING_RIGHT;
|
||||
|
||||
htd->base.name = XRT_DEVICE_HAND_TRACKER;
|
||||
|
||||
if (xp->tracking->create_tracked_hand(xp->tracking, &htd->base, &htd->tracker) < 0) {
|
||||
HT_DEBUG(htd, "Failed to create hand tracker module");
|
||||
ht_device_destroy(&htd->base);
|
||||
return NULL;
|
||||
}
|
||||
|
||||
u_hand_joints_init_default_set(&htd->u_tracking[XRT_HAND_LEFT], XRT_HAND_LEFT, XRT_HAND_TRACKING_MODEL_CAMERA,
|
||||
1.0);
|
||||
u_hand_joints_init_default_set(&htd->u_tracking[XRT_HAND_RIGHT], XRT_HAND_RIGHT, XRT_HAND_TRACKING_MODEL_CAMERA,
|
||||
1.0);
|
||||
|
||||
u_var_add_root(htd, "Camera based Hand Tracker", true);
|
||||
u_var_add_ro_text(htd, htd->base.str, "Name");
|
||||
|
||||
HT_DEBUG(htd, "Hand Tracker initialized!");
|
||||
|
||||
return &htd->base;
|
||||
}
|
||||
534
src/xrt/drivers/ht/ht_driver.cpp
Normal file
534
src/xrt/drivers/ht/ht_driver.cpp
Normal file
|
|
@ -0,0 +1,534 @@
|
|||
// Copyright 2021, Collabora, Ltd.
|
||||
// SPDX-License-Identifier: BSL-1.0
|
||||
/*!
|
||||
* @file
|
||||
* @brief Camera based hand tracking driver code.
|
||||
* @author Moses Turner <moses@collabora.com>
|
||||
* @author Jakob Bornecrantz <jakob@collabora.com>
|
||||
* @ingroup drv_ht
|
||||
*/
|
||||
|
||||
#include "ht_driver.hpp"
|
||||
#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 "tracking/t_frame_cv_mat_wrapper.hpp"
|
||||
#include <cjson/cJSON.h>
|
||||
#include <opencv2/core/mat.hpp>
|
||||
#include <unistd.h>
|
||||
|
||||
#include "templates/NaivePermutationSort.hpp"
|
||||
|
||||
#include "opencv2/calib3d.hpp"
|
||||
#include "opencv2/highgui.hpp"
|
||||
#include "os/os_threading.h"
|
||||
#include "os/os_time.h"
|
||||
#include "util/u_time.h"
|
||||
#include "util/u_json.h"
|
||||
#include "util/u_config_json.h"
|
||||
|
||||
#include "math/m_eigen_interop.hpp"
|
||||
|
||||
|
||||
|
||||
// #include <asm-generic/errno-base.h>
|
||||
|
||||
#include "vive/vive_config.h"
|
||||
|
||||
#include "tracking/t_calibration_opencv.hpp"
|
||||
|
||||
#include "util/u_logging.h"
|
||||
#include "util/u_time.h"
|
||||
#include "util/u_trace_marker.h"
|
||||
#include "xrt/xrt_defines.h"
|
||||
|
||||
#include "ht_algorithm.hpp"
|
||||
#include "xrt/xrt_frameserver.h"
|
||||
|
||||
// #include <opencv2/imgproc.hpp>
|
||||
#include <string.h>
|
||||
#include <stdio.h>
|
||||
#include <math.h>
|
||||
|
||||
#include <float.h>
|
||||
|
||||
|
||||
|
||||
#include <exception>
|
||||
#include <fstream>
|
||||
#include <iostream>
|
||||
#include <limits>
|
||||
#include <numeric>
|
||||
#include <cmath>
|
||||
#include <sstream>
|
||||
#include <algorithm>
|
||||
|
||||
#include <thread>
|
||||
#include <future>
|
||||
|
||||
/*!
|
||||
* Setup helper functions.
|
||||
*/
|
||||
|
||||
static bool
|
||||
getCalibration(struct ht_device *htd, t_stereo_camera_calibration *calibration)
|
||||
{
|
||||
xrt::auxiliary::tracking::StereoCameraCalibrationWrapper wrap(calibration);
|
||||
xrt_vec3 trans = {(float)wrap.camera_translation_mat(0, 0), (float)wrap.camera_translation_mat(1, 0),
|
||||
(float)wrap.camera_translation_mat(2, 0)};
|
||||
htd->baseline = m_vec3_len(trans);
|
||||
|
||||
#if 0
|
||||
std::cout << "\n\nTRANSLATION VECTOR IS\n" << wrap.camera_translation_mat;
|
||||
std::cout << "\n\nROTATION FROM LEFT TO RIGHT IS\n" << wrap.camera_rotation_mat << "\n";
|
||||
#endif
|
||||
|
||||
cv::Matx34d P1;
|
||||
cv::Matx34d P2;
|
||||
|
||||
cv::Matx44d Q;
|
||||
|
||||
// The only reason we're calling stereoRectify is because we want R1 and R2 for the
|
||||
cv::stereoRectify(wrap.view[0].intrinsics_mat, // cameraMatrix1
|
||||
wrap.view[0].distortion_mat, // distCoeffs1
|
||||
wrap.view[1].intrinsics_mat, // cameraMatrix2
|
||||
wrap.view[1].distortion_mat, // distCoeffs2
|
||||
cv::Size(960, 960), // imageSize
|
||||
wrap.camera_rotation_mat, // R
|
||||
wrap.camera_translation_mat, // T
|
||||
htd->views[0].rotate_camera_to_stereo_camera, // R1
|
||||
htd->views[1].rotate_camera_to_stereo_camera, // R2
|
||||
P1, // P1
|
||||
P2, // P2
|
||||
Q, // Q
|
||||
0, // flags
|
||||
-1.0f, // alpha
|
||||
cv::Size(), // newImageSize
|
||||
NULL, // validPixROI1
|
||||
NULL); // validPixROI2
|
||||
|
||||
|
||||
for (int i = 0; i < 2; i++) {
|
||||
htd->views[i].cameraMatrix = wrap.view[i].intrinsics_mat;
|
||||
|
||||
htd->views[i].distortion = wrap.view[i].distortion_fisheye_mat;
|
||||
}
|
||||
|
||||
cv::Matx33d rotate_stereo_camera_to_left_camera = htd->views[0].rotate_camera_to_stereo_camera.inv();
|
||||
|
||||
xrt_matrix_3x3 s;
|
||||
s.v[0] = rotate_stereo_camera_to_left_camera(0, 0);
|
||||
s.v[1] = rotate_stereo_camera_to_left_camera(0, 1);
|
||||
s.v[2] = rotate_stereo_camera_to_left_camera(0, 2);
|
||||
|
||||
s.v[3] = rotate_stereo_camera_to_left_camera(1, 0);
|
||||
s.v[4] = rotate_stereo_camera_to_left_camera(1, 1);
|
||||
s.v[5] = rotate_stereo_camera_to_left_camera(1, 2);
|
||||
|
||||
s.v[6] = rotate_stereo_camera_to_left_camera(2, 0);
|
||||
s.v[7] = rotate_stereo_camera_to_left_camera(2, 1);
|
||||
s.v[8] = rotate_stereo_camera_to_left_camera(2, 2);
|
||||
|
||||
xrt_quat tmp;
|
||||
|
||||
math_quat_from_matrix_3x3(&s, &tmp);
|
||||
|
||||
// Weird that I have to invert this quat, right? I think at some point - like probably just above this - I must
|
||||
// have swapped row-major and col-major - remember, if you transpose a rotation matrix, you get its inverse.
|
||||
// Doesn't matter that I don't understand - non-inverted looks definitely wrong, inverted looks definitely
|
||||
// right.
|
||||
math_quat_invert(&tmp, &htd->stereo_camera_to_left_camera);
|
||||
|
||||
#if 0
|
||||
U_LOG_E("%f %f %f %f", htd->stereo_camera_to_left_camera.w, htd->stereo_camera_to_left_camera.x,
|
||||
htd->stereo_camera_to_left_camera.y, htd->stereo_camera_to_left_camera.z);
|
||||
#endif
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
static void
|
||||
getUserConfig(struct ht_device *htd)
|
||||
{
|
||||
// The game here is to avoid bugs + be paranoid, not to be fast. If you see something that seems "slow" - don't
|
||||
// fix it. Any of the tracking code is way stickier than this could ever be.
|
||||
|
||||
// Set defaults
|
||||
// Admit defeat: for now, Mediapipe's are still better than ours.
|
||||
htd->runtime_config.palm_detection_use_mediapipe = true;
|
||||
htd->runtime_config.keypoint_estimation_use_mediapipe = true;
|
||||
|
||||
// Make sure you build DebugOptimized!
|
||||
htd->runtime_config.desired_format = XRT_FORMAT_YUYV422;
|
||||
|
||||
struct u_config_json config_json = {};
|
||||
|
||||
u_config_json_open_or_create_main_file(&config_json);
|
||||
if (!config_json.file_loaded) {
|
||||
return;
|
||||
}
|
||||
|
||||
cJSON *ht_config_json = cJSON_GetObjectItemCaseSensitive(config_json.root, "config_ht");
|
||||
if (ht_config_json == NULL) {
|
||||
return;
|
||||
}
|
||||
|
||||
cJSON *palm_detection_type = cJSON_GetObjectItemCaseSensitive(ht_config_json, "palm_detection_model");
|
||||
cJSON *keypoint_estimation_type = cJSON_GetObjectItemCaseSensitive(ht_config_json, "keypoint_estimation_model");
|
||||
cJSON *uvc_wire_format = cJSON_GetObjectItemCaseSensitive(ht_config_json, "uvc_wire_format");
|
||||
|
||||
// IsString does its own null-checking
|
||||
if (cJSON_IsString(palm_detection_type)) {
|
||||
bool is_collabora = (strcmp(palm_detection_type->valuestring, "collabora") == 0);
|
||||
bool is_mediapipe = (strcmp(palm_detection_type->valuestring, "mediapipe") == 0);
|
||||
if (!is_collabora && !is_mediapipe) {
|
||||
HT_WARN(htd, "Unknown palm detection type %s - should be \"collabora\" or \"mediapipe\"",
|
||||
palm_detection_type->valuestring);
|
||||
}
|
||||
htd->runtime_config.palm_detection_use_mediapipe = is_mediapipe;
|
||||
}
|
||||
|
||||
if (cJSON_IsString(keypoint_estimation_type)) {
|
||||
bool is_collabora = (strcmp(keypoint_estimation_type->valuestring, "collabora") == 0);
|
||||
bool is_mediapipe = (strcmp(keypoint_estimation_type->valuestring, "mediapipe") == 0);
|
||||
if (!is_collabora && !is_mediapipe) {
|
||||
HT_WARN(htd, "Unknown keypoint estimation type %s - should be \"collabora\" or \"mediapipe\"",
|
||||
keypoint_estimation_type->valuestring);
|
||||
}
|
||||
htd->runtime_config.keypoint_estimation_use_mediapipe = is_mediapipe;
|
||||
}
|
||||
|
||||
if (cJSON_IsString(uvc_wire_format)) {
|
||||
bool is_yuv = (strcmp(cJSON_GetStringValue(uvc_wire_format), "yuv") == 0);
|
||||
bool is_mjpeg = (strcmp(cJSON_GetStringValue(uvc_wire_format), "mjpeg") == 0);
|
||||
if (!is_yuv && !is_mjpeg) {
|
||||
HT_WARN(htd, "Unknown wire format type %s - should be \"yuv\" or \"mjpeg\"",
|
||||
cJSON_GetStringValue(uvc_wire_format));
|
||||
}
|
||||
if (is_yuv) {
|
||||
HT_DEBUG(htd, "Using YUYV422!");
|
||||
htd->runtime_config.desired_format = XRT_FORMAT_YUYV422;
|
||||
} else {
|
||||
HT_DEBUG(htd, "Using MJPEG!");
|
||||
htd->runtime_config.desired_format = XRT_FORMAT_MJPEG;
|
||||
}
|
||||
}
|
||||
|
||||
cJSON_Delete(config_json.root);
|
||||
return;
|
||||
}
|
||||
|
||||
|
||||
static void
|
||||
getModelsFolder(struct ht_device *htd)
|
||||
{
|
||||
// Please bikeshed me on this! I don't know where is the best place to put this stuff!
|
||||
#if 0
|
||||
char exec_location[1024] = {};
|
||||
readlink("/proc/self/exe", exec_location, 1024);
|
||||
|
||||
HT_DEBUG(htd, "Exec at %s\n", exec_location);
|
||||
|
||||
int end = 0;
|
||||
while (exec_location[end] != '\0') {
|
||||
HT_DEBUG(htd, "%d", end);
|
||||
end++;
|
||||
}
|
||||
|
||||
while (exec_location[end] != '/' && end != 0) {
|
||||
HT_DEBUG(htd, "%d %c", end, exec_location[end]);
|
||||
exec_location[end] = '\0';
|
||||
end--;
|
||||
}
|
||||
|
||||
strcat(exec_location, "../share/monado/hand-tracking-models/");
|
||||
strcpy(htd->runtime_config.model_slug, exec_location);
|
||||
#else
|
||||
const char *xdg_home = getenv("XDG_CONFIG_HOME");
|
||||
const char *home = getenv("HOME");
|
||||
if (xdg_home != NULL) {
|
||||
strcpy(htd->runtime_config.model_slug, xdg_home);
|
||||
} else if (home != NULL) {
|
||||
strcpy(htd->runtime_config.model_slug, home);
|
||||
} else {
|
||||
assert(false);
|
||||
}
|
||||
strcat(htd->runtime_config.model_slug, "/.local/share/monado/hand-tracking-models/");
|
||||
#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->camera.xfctx, &htd->camera.xfs);
|
||||
htd->found_camera = true;
|
||||
return;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/*!
|
||||
* xrt_frame_sink function implementations
|
||||
*/
|
||||
|
||||
static void
|
||||
ht_sink_push_frame(struct xrt_frame_sink *xs, struct xrt_frame *xf)
|
||||
{
|
||||
XRT_TRACE_MARKER();
|
||||
struct ht_device *htd = container_of(xs, struct ht_device, sink);
|
||||
assert(xf != NULL);
|
||||
|
||||
if (!htd->tracking_should_die) {
|
||||
os_mutex_lock(&htd->dying_breath);
|
||||
|
||||
xrt_frame_reference(&htd->frame_for_process, xf);
|
||||
htRunAlgorithm(htd);
|
||||
xrt_frame_reference(&htd->frame_for_process, NULL); // Could let go of it a little earlier but nah
|
||||
|
||||
os_mutex_unlock(&htd->dying_breath);
|
||||
}
|
||||
}
|
||||
|
||||
/*!
|
||||
* xrt_frame_node function implementations
|
||||
*/
|
||||
|
||||
static void
|
||||
ht_node_break_apart(struct xrt_frame_node *node)
|
||||
{
|
||||
struct ht_device *htd = container_of(node, struct ht_device, node);
|
||||
HT_DEBUG(htd, "called!");
|
||||
// wrong but don't care
|
||||
}
|
||||
|
||||
static void
|
||||
ht_node_destroy(struct xrt_frame_node *node)
|
||||
{
|
||||
struct ht_device *htd = container_of(node, struct ht_device, node);
|
||||
|
||||
HT_DEBUG(htd, "called!");
|
||||
}
|
||||
|
||||
/*!
|
||||
* xrt_device function implementations
|
||||
*/
|
||||
|
||||
static void
|
||||
ht_device_update_inputs(struct xrt_device *xdev)
|
||||
{
|
||||
// Empty
|
||||
}
|
||||
|
||||
static void
|
||||
ht_device_get_hand_tracking(struct xrt_device *xdev,
|
||||
enum xrt_input_name name,
|
||||
uint64_t at_timestamp_ns,
|
||||
struct xrt_hand_joint_set *out_value)
|
||||
{
|
||||
// Note! Currently, this totally ignores at_timestamp_ns. We need a better interface.
|
||||
struct ht_device *htd = ht_device(xdev);
|
||||
|
||||
if (name != XRT_INPUT_GENERIC_HAND_TRACKING_LEFT && name != XRT_INPUT_GENERIC_HAND_TRACKING_RIGHT) {
|
||||
HT_ERROR(htd, "unknown input name for hand tracker");
|
||||
return;
|
||||
}
|
||||
bool hand_index = (name == XRT_INPUT_GENERIC_HAND_TRACKING_RIGHT); // left=0, right=1
|
||||
|
||||
|
||||
|
||||
os_mutex_lock(&htd->openxr_hand_data_mediator);
|
||||
memcpy(out_value, &htd->hands_for_openxr[hand_index], sizeof(struct xrt_hand_joint_set));
|
||||
os_mutex_unlock(&htd->openxr_hand_data_mediator);
|
||||
}
|
||||
|
||||
static void
|
||||
ht_device_destroy(struct xrt_device *xdev)
|
||||
{
|
||||
struct ht_device *htd = ht_device(xdev);
|
||||
HT_DEBUG(htd, "called!");
|
||||
|
||||
|
||||
xrt_frame_context_destroy_nodes(&htd->camera.xfctx);
|
||||
htd->tracking_should_die = true;
|
||||
|
||||
// Lock this mutex so we don't try to free things as they're being used on the last iteration
|
||||
os_mutex_lock(&htd->dying_breath);
|
||||
destroyOnnx(htd);
|
||||
#if defined(JSON_OUTPUT)
|
||||
const char *string = cJSON_Print(htd->output_root);
|
||||
FILE *fp;
|
||||
fp = fopen("/1/2handtrack/aug12.json", "w");
|
||||
|
||||
|
||||
fprintf(fp, "%s", string);
|
||||
fclose(fp);
|
||||
cJSON_Delete(htd->output_root);
|
||||
#endif
|
||||
|
||||
// Remove the variable tracking.
|
||||
u_var_remove_root(htd);
|
||||
|
||||
// Shhhhhhhhhhh, it's okay. It'll all be okay.
|
||||
htd->histories_3d.~vector();
|
||||
htd->views[0].bbox_histories.~vector();
|
||||
htd->views[1].bbox_histories.~vector();
|
||||
// Okay, fine, since we're mixing C and C++ idioms here, I couldn't find a clean way to implicitly
|
||||
// call the destructors on these (ht_device doesn't have a destructor; neither do most of its members; and if
|
||||
// you read u_device_allocate and u_device_free you'll agree it'd be somewhat annoying to write a
|
||||
// constructor/destructor for ht_device), so we just manually call the destructors for things like std::vector's
|
||||
// that need their destructors to be called to not leak.
|
||||
|
||||
u_device_free(&htd->base);
|
||||
}
|
||||
|
||||
extern "C" struct xrt_device *
|
||||
ht_device_create(struct xrt_prober *xp, struct t_stereo_camera_calibration *calib)
|
||||
{
|
||||
XRT_TRACE_MARKER();
|
||||
enum u_device_alloc_flags flags = U_DEVICE_ALLOC_NO_FLAGS;
|
||||
|
||||
//! @todo 2 hands hardcoded
|
||||
int num_hands = 2;
|
||||
|
||||
// Allocate device
|
||||
struct ht_device *htd = U_DEVICE_ALLOCATE(struct ht_device, flags, num_hands, 0);
|
||||
|
||||
// Setup logging first. We like logging.
|
||||
htd->ll = debug_get_log_option_ht_log();
|
||||
|
||||
// Get configuration
|
||||
assert(calib != NULL);
|
||||
getCalibration(htd, calib);
|
||||
getUserConfig(htd);
|
||||
getModelsFolder(htd);
|
||||
|
||||
// Add xrt_frame_sink and xrt_frame_node implementations
|
||||
htd->sink.push_frame = &ht_sink_push_frame;
|
||||
htd->node.break_apart = &ht_node_break_apart;
|
||||
htd->node.destroy = &ht_node_destroy;
|
||||
|
||||
// Add ourselves to the frame context
|
||||
xrt_frame_context_add(&htd->camera.xfctx, &htd->node);
|
||||
|
||||
|
||||
htd->camera.one_view_size_px.w = 960;
|
||||
htd->camera.one_view_size_px.h = 960;
|
||||
htd->camera.prober = xp;
|
||||
xrt_prober_list_video_devices(htd->camera.prober, on_video_device, htd);
|
||||
|
||||
|
||||
if (!htd->found_camera) {
|
||||
return NULL;
|
||||
}
|
||||
|
||||
|
||||
htd->views[0].htd = htd;
|
||||
htd->views[1].htd = htd; // :)
|
||||
|
||||
htd->views[0].view = 0;
|
||||
htd->views[1].view = 1;
|
||||
|
||||
|
||||
initOnnx(htd);
|
||||
|
||||
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;
|
||||
|
||||
os_mutex_init(&htd->openxr_hand_data_mediator);
|
||||
os_mutex_init(&htd->dying_breath);
|
||||
|
||||
htd->base.update_inputs = ht_device_update_inputs;
|
||||
htd->base.get_hand_tracking = ht_device_get_hand_tracking;
|
||||
htd->base.destroy = ht_device_destroy;
|
||||
|
||||
snprintf(htd->base.str, XRT_DEVICE_NAME_LEN, "Camera based Hand Tracker");
|
||||
snprintf(htd->base.serial, XRT_DEVICE_NAME_LEN, "Camera based Hand Tracker");
|
||||
|
||||
htd->base.inputs[0].name = XRT_INPUT_GENERIC_HAND_TRACKING_LEFT;
|
||||
htd->base.inputs[1].name = XRT_INPUT_GENERIC_HAND_TRACKING_RIGHT;
|
||||
|
||||
// Yes, you need all of these. Yes, I tried disabling them all one at a time. You need all of these.
|
||||
htd->base.name = XRT_DEVICE_HAND_TRACKER;
|
||||
htd->base.device_type = XRT_DEVICE_TYPE_HAND_TRACKER;
|
||||
htd->base.orientation_tracking_supported = true;
|
||||
htd->base.position_tracking_supported = true;
|
||||
htd->base.hand_tracking_supported = true;
|
||||
|
||||
#if defined(JSON_OUTPUT)
|
||||
htd->output_root = cJSON_CreateObject();
|
||||
htd->output_array = cJSON_CreateArray();
|
||||
cJSON_AddItemToObject(htd->output_root, "hand_array", htd->output_array);
|
||||
#endif
|
||||
|
||||
struct xrt_frame_sink *tmp = &htd->sink;
|
||||
|
||||
u_var_add_root(htd, "Camera based Hand Tracker", true);
|
||||
u_var_add_ro_text(htd, htd->base.str, "Name");
|
||||
|
||||
// This puts u_sink_create_to_r8g8b8_or_l8 on its own thread, so that nothing gets backed up if it runs slower
|
||||
// than the native camera framerate.
|
||||
u_sink_queue_create(&htd->camera.xfctx, tmp, &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!
|
||||
u_sink_create_to_r8g8b8_or_l8(&htd->camera.xfctx, tmp, &tmp);
|
||||
|
||||
// Puts the hand tracking code on its own thread, so that nothing upstream of it gets backed up if the hand
|
||||
// tracking code runs slower than the upstream framerate.
|
||||
u_sink_queue_create(&htd->camera.xfctx, tmp, &tmp);
|
||||
|
||||
xrt_fs_mode *modes;
|
||||
uint32_t count;
|
||||
|
||||
xrt_fs_enumerate_modes(htd->camera.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->runtime_config.desired_format) {
|
||||
found_mode = true;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
if (!found_mode) {
|
||||
selected_mode = 0;
|
||||
HT_WARN(htd, "Couldn't find desired camera mode! Something's probably wrong.");
|
||||
}
|
||||
|
||||
free(modes);
|
||||
|
||||
|
||||
xrt_fs_stream_start(htd->camera.xfs, tmp, XRT_FS_CAPTURE_TYPE_TRACKING, selected_mode);
|
||||
|
||||
#if 1
|
||||
u_var_add_sink(htd, &htd->debug_sink, "Debug visualization");
|
||||
#endif
|
||||
|
||||
|
||||
HT_DEBUG(htd, "Hand Tracker initialized!");
|
||||
|
||||
|
||||
return &htd->base;
|
||||
}
|
||||
|
|
@ -1,26 +0,0 @@
|
|||
// Copyright 2020, Collabora, Ltd.
|
||||
// SPDX-License-Identifier: BSL-1.0
|
||||
/*!
|
||||
* @file
|
||||
* @brief Interface to camera based hand tracking driver code.
|
||||
* @author Christtoph Haag <christtoph.haag@collabora.com>
|
||||
* @ingroup drv_ht
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "math/m_api.h"
|
||||
#include "xrt/xrt_device.h"
|
||||
#include "xrt/xrt_prober.h"
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
struct xrt_device *
|
||||
ht_device_create();
|
||||
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
290
src/xrt/drivers/ht/ht_driver.hpp
Normal file
290
src/xrt/drivers/ht/ht_driver.hpp
Normal file
|
|
@ -0,0 +1,290 @@
|
|||
// Copyright 2021, Collabora, Ltd.
|
||||
// SPDX-License-Identifier: BSL-1.0
|
||||
/*!
|
||||
* @file
|
||||
* @brief Defines and common includes for camera-based hand tracker
|
||||
* @author Moses Turner <moses@collabora.com>
|
||||
* @ingroup drv_ht
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
|
||||
#include "math/m_api.h"
|
||||
#include "math/m_vec3.h"
|
||||
|
||||
#include "math/m_filter_one_euro.h"
|
||||
|
||||
#include "xrt/xrt_device.h"
|
||||
#include "xrt/xrt_prober.h"
|
||||
#include "xrt/xrt_frame.h"
|
||||
#include "xrt/xrt_frameserver.h"
|
||||
|
||||
#include "util/u_var.h"
|
||||
#include "util/u_debug.h"
|
||||
#include "util/u_sink.h"
|
||||
#include "util/u_device.h"
|
||||
|
||||
#include "os/os_threading.h"
|
||||
|
||||
#include <future>
|
||||
#include <opencv2/opencv.hpp>
|
||||
|
||||
#include "core/session/onnxruntime_c_api.h"
|
||||
|
||||
#include "templates/DiscardLastBuffer.hpp"
|
||||
|
||||
#include "util/u_json.h"
|
||||
|
||||
#include <vector>
|
||||
|
||||
|
||||
|
||||
DEBUG_GET_ONCE_LOG_OPTION(ht_log, "HT_LOG", U_LOGGING_WARN)
|
||||
|
||||
#define HT_TRACE(htd, ...) U_LOG_XDEV_IFL_T(&htd->base, htd->ll, __VA_ARGS__)
|
||||
#define HT_DEBUG(htd, ...) U_LOG_XDEV_IFL_D(&htd->base, htd->ll, __VA_ARGS__)
|
||||
#define HT_INFO(htd, ...) U_LOG_XDEV_IFL_I(&htd->base, htd->ll, __VA_ARGS__)
|
||||
#define HT_WARN(htd, ...) U_LOG_XDEV_IFL_W(&htd->base, htd->ll, __VA_ARGS__)
|
||||
#define HT_ERROR(htd, ...) U_LOG_XDEV_IFL_E(&htd->base, htd->ll, __VA_ARGS__)
|
||||
|
||||
// #define ht_
|
||||
|
||||
|
||||
// To make clang-tidy happy
|
||||
#define opencv_distortion_param_num 4
|
||||
|
||||
/*
|
||||
*
|
||||
* Compile-time defines to choose where to get camera frames from and what kind of output to give out
|
||||
*
|
||||
*/
|
||||
#undef JSON_OUTPUT
|
||||
|
||||
#define FCMIN_BBOX 3.0f
|
||||
#define FCMIN_D_BB0X 10.0f
|
||||
#define BETA_BB0X 0.0f
|
||||
|
||||
|
||||
#define FCMIN_HAND 4.0f
|
||||
#define FCMIN_D_HAND 12.0f
|
||||
#define BETA_HAND 0.05f
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
enum HandJoint7Keypoint
|
||||
{
|
||||
WRIST_7KP = 0,
|
||||
INDEX_7KP = 1,
|
||||
MIDDLE_7KP = 2,
|
||||
RING_7KP = 3,
|
||||
LITTLE_7KP = 4,
|
||||
THUMB_METACARPAL_7KP = 5,
|
||||
THMB_PROXIMAL_7KP = 6
|
||||
};
|
||||
|
||||
enum HandJoint21Keypoint
|
||||
{
|
||||
WRIST = 0,
|
||||
|
||||
THMB_MCP = 1,
|
||||
THMB_PXM = 2,
|
||||
THMB_DST = 3,
|
||||
THMB_TIP = 4,
|
||||
|
||||
INDX_PXM = 5,
|
||||
INDX_INT = 6,
|
||||
INDX_DST = 7,
|
||||
INDX_TIP = 8,
|
||||
|
||||
MIDL_PXM = 9,
|
||||
MIDL_INT = 10,
|
||||
MIDL_DST = 11,
|
||||
MIDL_TIP = 12,
|
||||
|
||||
RING_PXM = 13,
|
||||
RING_INT = 14,
|
||||
RING_DST = 15,
|
||||
RING_TIP = 16,
|
||||
|
||||
LITL_PXM = 17,
|
||||
LITL_INT = 18,
|
||||
LITL_DST = 19,
|
||||
LITL_TIP = 20
|
||||
};
|
||||
|
||||
struct Palm7KP
|
||||
{
|
||||
struct xrt_vec2 kps[7];
|
||||
};
|
||||
|
||||
// To keep you on your toes. *Don't* think the 2D hand is the same as the 3D!
|
||||
struct Hand2D
|
||||
{
|
||||
struct xrt_vec3 kps[21];
|
||||
// Third value is depth from ML model. Do not believe the depth.
|
||||
};
|
||||
|
||||
struct Hand3D
|
||||
{
|
||||
struct xrt_vec3 kps[21];
|
||||
float y_disparity_error;
|
||||
float flow_error;
|
||||
float handedness;
|
||||
uint64_t timestamp;
|
||||
};
|
||||
|
||||
|
||||
struct DetectionModelOutput
|
||||
{
|
||||
float rotation;
|
||||
float size;
|
||||
xrt_vec2 center;
|
||||
xrt_vec2 wrist;
|
||||
cv::Matx23f warp_there;
|
||||
cv::Matx23f warp_back;
|
||||
};
|
||||
|
||||
struct HandHistory3D
|
||||
{
|
||||
// Index 0 is current frame, index 1 is last frame, index 2 is second to last frame.
|
||||
// No particular reason to keep the last 5 frames. we only really only use the current and last one.
|
||||
float handedness;
|
||||
DiscardLastBuffer<Hand3D, 5> last_hands;
|
||||
// Euro filter for 21kps.
|
||||
m_filter_euro_vec3 filters[21];
|
||||
};
|
||||
|
||||
struct HandHistory2DBBox
|
||||
{
|
||||
m_filter_euro_vec2 m_filter_wrist;
|
||||
m_filter_euro_vec2 m_filter_middle;
|
||||
|
||||
DiscardLastBuffer<xrt_vec2, 50> wrist_unfiltered;
|
||||
DiscardLastBuffer<xrt_vec2, 50> middle_unfiltered;
|
||||
};
|
||||
|
||||
|
||||
struct ModelInfo
|
||||
{
|
||||
OrtSession *session = nullptr;
|
||||
OrtMemoryInfo *memoryInfo = nullptr;
|
||||
// std::vector's don't make too much sense here, but they're oh so easy
|
||||
std::vector<int64_t> input_shape;
|
||||
size_t input_size_bytes;
|
||||
std::vector<const char *> output_names;
|
||||
std::vector<const char *> input_names;
|
||||
};
|
||||
|
||||
|
||||
// Forward declaration for ht_view
|
||||
struct ht_device;
|
||||
|
||||
struct ht_view
|
||||
{
|
||||
ht_device *htd;
|
||||
int view; // :)))
|
||||
|
||||
// Loaded from config file
|
||||
cv::Matx<double, opencv_distortion_param_num, 1> distortion;
|
||||
cv::Matx<double, 3, 3> cameraMatrix;
|
||||
cv::Matx33d rotate_camera_to_stereo_camera; // R1 or R2
|
||||
|
||||
cv::Mat run_model_on_this;
|
||||
cv::Mat debug_out_to_this;
|
||||
|
||||
std::vector<HandHistory2DBBox> bbox_histories;
|
||||
|
||||
struct ModelInfo detection_model;
|
||||
std::vector<Palm7KP> (*run_detection_model)(struct ht_view *htv, cv::Mat &img);
|
||||
|
||||
struct ModelInfo keypoint_model;
|
||||
// The cv::mat is passed by value, *not* passed by reference or by pointer;
|
||||
// in the tight loop that sets these off we reuse that cv::Mat; changing the data pointer as all the models are
|
||||
// running is... going to wreak havoc let's say that.
|
||||
Hand2D (*run_keypoint_model)(struct ht_view *htv, cv::Mat img);
|
||||
};
|
||||
|
||||
struct ht_device
|
||||
{
|
||||
struct xrt_device base;
|
||||
|
||||
struct xrt_tracking_origin tracking_origin; // probably cargo-culted
|
||||
|
||||
struct xrt_frame_sink sink;
|
||||
struct xrt_frame_node node;
|
||||
|
||||
struct xrt_frame_sink *debug_sink; // this must be bad.
|
||||
|
||||
|
||||
struct
|
||||
{
|
||||
struct xrt_frame_context xfctx;
|
||||
|
||||
struct xrt_fs *xfs;
|
||||
|
||||
struct xrt_fs_mode mode;
|
||||
|
||||
struct xrt_prober *prober;
|
||||
|
||||
struct xrt_size one_view_size_px;
|
||||
} camera;
|
||||
|
||||
bool found_camera;
|
||||
|
||||
const OrtApi *ort_api;
|
||||
OrtEnv *ort_env;
|
||||
|
||||
struct xrt_frame *frame_for_process;
|
||||
|
||||
struct ht_view views[2];
|
||||
|
||||
// These are all we need - R and T don't aren't of interest to us.
|
||||
// [2];
|
||||
float baseline;
|
||||
|
||||
struct xrt_quat stereo_camera_to_left_camera;
|
||||
|
||||
uint64_t current_frame_timestamp; // SUPER dumb.
|
||||
|
||||
std::vector<HandHistory3D> histories_3d;
|
||||
|
||||
struct os_mutex openxr_hand_data_mediator;
|
||||
struct xrt_hand_joint_set hands_for_openxr[2];
|
||||
|
||||
bool tracking_should_die;
|
||||
struct os_mutex dying_breath;
|
||||
|
||||
bool debug_scribble = true;
|
||||
|
||||
|
||||
#if defined(JSON_OUTPUT)
|
||||
cJSON *output_root;
|
||||
cJSON *output_array;
|
||||
#endif
|
||||
|
||||
struct
|
||||
{
|
||||
bool palm_detection_use_mediapipe;
|
||||
bool keypoint_estimation_use_mediapipe;
|
||||
enum xrt_format desired_format;
|
||||
char model_slug[1024];
|
||||
} runtime_config;
|
||||
|
||||
|
||||
|
||||
enum u_logging_level ll;
|
||||
};
|
||||
|
||||
static inline struct ht_device *
|
||||
ht_device(struct xrt_device *xdev)
|
||||
{
|
||||
return (struct ht_device *)xdev;
|
||||
}
|
||||
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
307
src/xrt/drivers/ht/ht_hand_math.hpp
Normal file
307
src/xrt/drivers/ht/ht_hand_math.hpp
Normal file
|
|
@ -0,0 +1,307 @@
|
|||
// Copyright 2021, Collabora, Ltd.
|
||||
// SPDX-License-Identifier: BSL-1.0
|
||||
/*!
|
||||
* @file
|
||||
* @brief Helper math to do things with 3D hands for the camera-based hand tracker
|
||||
* @author Moses Turner <moses@collabora.com>
|
||||
* @author Nick Klingensmith <programmerpichu@gmail.com>
|
||||
* @ingroup drv_ht
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
#include "ht_driver.hpp"
|
||||
#include "math/m_api.h"
|
||||
#include "math/m_vec3.h"
|
||||
|
||||
const int num_real_joints = 21;
|
||||
|
||||
static float
|
||||
errHandDisparity(Hand2D *left_rays, Hand2D *right_rays)
|
||||
{
|
||||
float error = 0.0f;
|
||||
for (int i = 0; i < 21; i++) {
|
||||
float diff = fabsf(left_rays->kps[i].y - right_rays->kps[i].y);
|
||||
// Big question about what's the best loss function. Gut feeling was "I should be using sum of squared
|
||||
// errors" but I don't really know. Using just sum of errors for now. Ideally it'd also be not very
|
||||
// sensitive to one or two really bad outliers.
|
||||
error += diff;
|
||||
}
|
||||
return error;
|
||||
}
|
||||
|
||||
|
||||
static float
|
||||
errHandFlow(Hand3D *prev, Hand3D *next)
|
||||
{
|
||||
float error = 0.0f;
|
||||
for (int i = 0; i < num_real_joints; i++) {
|
||||
xrt_vec3 first = prev->kps[i];
|
||||
xrt_vec3 second = next->kps[i];
|
||||
error += m_vec3_len(m_vec3_sub(second, first));
|
||||
}
|
||||
return error;
|
||||
}
|
||||
|
||||
static float
|
||||
errHandHistory(HandHistory3D *history_hand, Hand3D *present_hand)
|
||||
{
|
||||
// Remember we never have to deal with an empty hand. Can always access the last element.
|
||||
return errHandFlow(history_hand->last_hands[0], present_hand);
|
||||
}
|
||||
|
||||
|
||||
static void
|
||||
applyJointWidths(struct xrt_hand_joint_set *set)
|
||||
{
|
||||
// Thanks to Nick Klingensmith for this idea
|
||||
struct xrt_hand_joint_value *gr = set->values.hand_joint_set_default;
|
||||
|
||||
const float hand_joint_size[5] = {0.022f, 0.021f, 0.022f, 0.021f, 0.02f};
|
||||
const float hand_finger_size[5] = {1.0f, 1.0f, 0.83f, 0.75f};
|
||||
|
||||
const float thumb_size[4] = {0.016f, 0.014f, 0.012f, 0.012f};
|
||||
float mul = 1.0f;
|
||||
|
||||
for (int i = XRT_HAND_JOINT_THUMB_METACARPAL; i <= XRT_HAND_JOINT_THUMB_TIP; i++) {
|
||||
int j = i - XRT_HAND_JOINT_THUMB_METACARPAL;
|
||||
gr[i].radius = thumb_size[j] * mul;
|
||||
}
|
||||
|
||||
for (int finger = 0; finger < 4; finger++) {
|
||||
for (int joint = 0; joint < 5; joint++) {
|
||||
int set_idx = finger * 5 + joint + XRT_HAND_JOINT_INDEX_METACARPAL;
|
||||
float val = hand_joint_size[joint] * hand_finger_size[finger] * .5 * mul;
|
||||
gr[set_idx].radius = val;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
static void
|
||||
applyThumbIndexDrag(Hand3D *hand)
|
||||
{
|
||||
// TERRIBLE HACK.
|
||||
// Puts the thumb and pointer a bit closer together to be better at triggering XR clients' pinch detection.
|
||||
const float max_radius = 0.09; // 9 centimeters.
|
||||
const float min_radius = 0.00;
|
||||
|
||||
// no min drag, min drag always 0.
|
||||
const float max_drag = 0.75f;
|
||||
|
||||
xrt_vec3 thumb = hand->kps[THMB_TIP];
|
||||
xrt_vec3 index = hand->kps[INDX_TIP];
|
||||
xrt_vec3 ttp = index - thumb;
|
||||
float length = m_vec3_len(ttp);
|
||||
if ((length > max_radius)) {
|
||||
return;
|
||||
}
|
||||
|
||||
|
||||
float amount = math_map_ranges(length, min_radius, max_radius, max_drag, 0.0f);
|
||||
|
||||
hand->kps[THMB_TIP] = m_vec3_lerp(thumb, index, amount * 0.5f);
|
||||
hand->kps[INDX_TIP] = m_vec3_lerp(index, thumb, amount * 0.5f);
|
||||
}
|
||||
|
||||
static void
|
||||
applyJointOrientations(struct xrt_hand_joint_set *set, bool is_right)
|
||||
{
|
||||
// The real rule to follow is that each joint's "X" axis is along the axis along which it can bend.
|
||||
// The nature of our estimation makes this a bit difficult, but these should work okay-ish under perfect
|
||||
// conditions
|
||||
if (set->is_active == false) {
|
||||
return;
|
||||
}
|
||||
|
||||
#define gl(jt) set->values.hand_joint_set_default[jt].relation.pose.position
|
||||
|
||||
xrt_vec3 pinky_prox = gl(XRT_HAND_JOINT_LITTLE_PROXIMAL);
|
||||
|
||||
xrt_vec3 index_prox = gl(XRT_HAND_JOINT_INDEX_PROXIMAL);
|
||||
|
||||
|
||||
xrt_vec3 pinky_to_index_prox = m_vec3_normalize(index_prox - pinky_prox);
|
||||
if (is_right) {
|
||||
pinky_to_index_prox = m_vec3_mul_scalar(pinky_to_index_prox, -1.0f);
|
||||
}
|
||||
|
||||
std::vector<std::vector<enum xrt_hand_joint>> fingers_with_joints_in_them = {
|
||||
|
||||
{XRT_HAND_JOINT_INDEX_METACARPAL, XRT_HAND_JOINT_INDEX_PROXIMAL, XRT_HAND_JOINT_INDEX_INTERMEDIATE,
|
||||
XRT_HAND_JOINT_INDEX_DISTAL, XRT_HAND_JOINT_INDEX_TIP},
|
||||
|
||||
{XRT_HAND_JOINT_MIDDLE_METACARPAL, XRT_HAND_JOINT_MIDDLE_PROXIMAL, XRT_HAND_JOINT_MIDDLE_INTERMEDIATE,
|
||||
XRT_HAND_JOINT_MIDDLE_DISTAL, XRT_HAND_JOINT_MIDDLE_TIP},
|
||||
|
||||
{XRT_HAND_JOINT_RING_METACARPAL, XRT_HAND_JOINT_RING_PROXIMAL, XRT_HAND_JOINT_RING_INTERMEDIATE,
|
||||
XRT_HAND_JOINT_RING_DISTAL, XRT_HAND_JOINT_RING_TIP},
|
||||
|
||||
{XRT_HAND_JOINT_LITTLE_METACARPAL, XRT_HAND_JOINT_LITTLE_PROXIMAL, XRT_HAND_JOINT_LITTLE_INTERMEDIATE,
|
||||
XRT_HAND_JOINT_LITTLE_DISTAL, XRT_HAND_JOINT_LITTLE_TIP},
|
||||
|
||||
};
|
||||
for (std::vector<enum xrt_hand_joint> finger : fingers_with_joints_in_them) {
|
||||
|
||||
for (int i = 0; i < 4; i++) {
|
||||
// Don't do fingertips. (Fingertip would be index 4.)
|
||||
struct xrt_vec3 forwards = m_vec3_normalize(gl(finger[i + 1]) - gl(finger[i]));
|
||||
struct xrt_vec3 backwards = m_vec3_mul_scalar(forwards, -1.0f);
|
||||
|
||||
struct xrt_vec3 left = m_vec3_orthonormalize(forwards, pinky_to_index_prox);
|
||||
// float dot = m_vec3_dot(backwards, left);
|
||||
// assert((m_vec3_dot(backwards,left) == 0.0f));
|
||||
math_quat_from_plus_x_z(
|
||||
&left, &backwards,
|
||||
&set->values.hand_joint_set_default[finger[i]].relation.pose.orientation);
|
||||
}
|
||||
// Do fingertip! Per XR_EXT_hand_tracking, just copy the distal joint's orientation. Doing anything else
|
||||
// is wrong.
|
||||
set->values.hand_joint_set_default[finger[4]].relation.pose.orientation =
|
||||
set->values.hand_joint_set_default[finger[3]].relation.pose.orientation;
|
||||
}
|
||||
|
||||
// wrist!
|
||||
// Not the best but acceptable. Eventually, probably, do triangle of wrist pinky prox and index prox.
|
||||
set->values.hand_joint_set_default[XRT_HAND_JOINT_WRIST].relation.pose.orientation =
|
||||
set->values.hand_joint_set_default[XRT_HAND_JOINT_MIDDLE_METACARPAL].relation.pose.orientation;
|
||||
|
||||
|
||||
// palm!
|
||||
set->values.hand_joint_set_default[XRT_HAND_JOINT_PALM].relation.pose.orientation =
|
||||
set->values.hand_joint_set_default[XRT_HAND_JOINT_MIDDLE_METACARPAL].relation.pose.orientation;
|
||||
|
||||
// thumb!
|
||||
// When I look at Ultraleap tracking, there's like, a "plane" made by the tip, distal and proximal (and kinda
|
||||
// MCP, but least squares fitting a plane is too hard for my baby brain) Normal to this plane is the +X, and
|
||||
// obviously forwards to the next joint is the -Z.
|
||||
xrt_vec3 thumb_prox_to_dist = gl(XRT_HAND_JOINT_THUMB_DISTAL) - gl(XRT_HAND_JOINT_THUMB_PROXIMAL);
|
||||
xrt_vec3 thumb_dist_to_tip = gl(XRT_HAND_JOINT_THUMB_TIP) - gl(XRT_HAND_JOINT_THUMB_DISTAL);
|
||||
xrt_vec3 plane_normal;
|
||||
if (!is_right) {
|
||||
math_vec3_cross(&thumb_prox_to_dist, &thumb_dist_to_tip, &plane_normal);
|
||||
} else {
|
||||
math_vec3_cross(&thumb_dist_to_tip, &thumb_prox_to_dist, &plane_normal);
|
||||
}
|
||||
std::vector<enum xrt_hand_joint> thumbs = {XRT_HAND_JOINT_THUMB_METACARPAL, XRT_HAND_JOINT_THUMB_PROXIMAL,
|
||||
XRT_HAND_JOINT_THUMB_DISTAL, XRT_HAND_JOINT_THUMB_TIP};
|
||||
for (int i = 0; i < 3; i++) {
|
||||
struct xrt_vec3 backwards =
|
||||
m_vec3_mul_scalar(m_vec3_normalize(gl(thumbs[i + 1]) - gl(thumbs[i])), -1.0f);
|
||||
|
||||
struct xrt_vec3 left = m_vec3_orthonormalize(backwards, plane_normal);
|
||||
math_quat_from_plus_x_z(&left, &backwards,
|
||||
&set->values.hand_joint_set_default[thumbs[i]].relation.pose.orientation);
|
||||
}
|
||||
struct xrt_quat *tip = &set->values.hand_joint_set_default[XRT_HAND_JOINT_THUMB_TIP].relation.pose.orientation;
|
||||
struct xrt_quat *distal =
|
||||
&set->values.hand_joint_set_default[XRT_HAND_JOINT_THUMB_DISTAL].relation.pose.orientation;
|
||||
memcpy(tip, distal, sizeof(struct xrt_quat));
|
||||
}
|
||||
|
||||
static float
|
||||
handednessJointSet(Hand3D *set)
|
||||
{
|
||||
// Guess if hand is left or right.
|
||||
// Left is negative, right is positive.
|
||||
|
||||
|
||||
// xrt_vec3 middle_mcp = gl(XRT_HAND_JOINT_MIDDLE_METACARPAL);
|
||||
|
||||
xrt_vec3 pinky_prox = set->kps[LITL_PXM]; // gl(XRT_HAND_JOINT_LITTLE_PROXIMAL);
|
||||
|
||||
xrt_vec3 index_prox = set->kps[INDX_PXM]; // gl(XRT_HAND_JOINT_INDEX_PROXIMAL);
|
||||
|
||||
xrt_vec3 pinky_to_index_prox = m_vec3_normalize(index_prox - pinky_prox);
|
||||
|
||||
float handedness = 0.0f;
|
||||
|
||||
for (int i : {INDX_PXM, MIDL_PXM, RING_PXM, LITL_PXM}) {
|
||||
xrt_vec3 prox = set->kps[i];
|
||||
xrt_vec3 intr = set->kps[i + 1];
|
||||
xrt_vec3 dist = set->kps[i + 2];
|
||||
xrt_vec3 tip = set->kps[i + 3];
|
||||
|
||||
xrt_vec3 prox_to_int = m_vec3_normalize(intr - prox);
|
||||
xrt_vec3 int_to_dist = m_vec3_normalize(dist - intr);
|
||||
xrt_vec3 dist_to_tip = m_vec3_normalize(tip - dist);
|
||||
|
||||
xrt_vec3 checks[2];
|
||||
|
||||
math_vec3_cross(&prox_to_int, &int_to_dist, &checks[0]);
|
||||
math_vec3_cross(&int_to_dist, &dist_to_tip, &checks[1]);
|
||||
|
||||
handedness += m_vec3_dot(m_vec3_normalize(pinky_to_index_prox), (checks[0]));
|
||||
handedness += m_vec3_dot(m_vec3_normalize(pinky_to_index_prox), (checks[1]));
|
||||
}
|
||||
set->handedness = handedness / (4 * 2);
|
||||
return set->handedness;
|
||||
}
|
||||
|
||||
static void
|
||||
handednessHandHistory3D(HandHistory3D *history)
|
||||
{
|
||||
|
||||
float inter = handednessJointSet(history->last_hands[0]);
|
||||
|
||||
if ((fabsf(inter) > 0.3f) || (fabsf(history->handedness) < 0.3f)) {
|
||||
history->handedness += inter;
|
||||
}
|
||||
const int max_handedness = 2.0f;
|
||||
if (history->handedness > max_handedness) {
|
||||
history->handedness = max_handedness;
|
||||
} else if (history->handedness < -max_handedness) {
|
||||
history->handedness = -max_handedness;
|
||||
}
|
||||
}
|
||||
|
||||
static void
|
||||
handEuroFiltersInit(HandHistory3D *history, double fc_min, double fc_min_d, double beta)
|
||||
{
|
||||
for (int i = 0; i < 21; i++) {
|
||||
m_filter_euro_vec3_init(&history->filters[i], fc_min, beta, fc_min_d);
|
||||
}
|
||||
}
|
||||
|
||||
static Hand3D
|
||||
handEuroFiltersRun(HandHistory3D *history)
|
||||
{
|
||||
// Assume present hand is in element 0!
|
||||
Hand3D hand;
|
||||
for (int i = 0; i < 21; i++) {
|
||||
m_filter_euro_vec3_run(&history->filters[i], history->last_hands[0]->timestamp,
|
||||
&history->last_hands[0]->kps[i], &hand.kps[i]);
|
||||
}
|
||||
return hand;
|
||||
}
|
||||
|
||||
static bool
|
||||
rejectTooFarOrTooClose(Hand3D *hand)
|
||||
{
|
||||
const float max_dist_from_camera_sqrd =
|
||||
2.f * 2.f; // If you ever run into somebody with 2-meter-long arms, let me know!
|
||||
const float min_dist_from_camera_sqrd = 0.05f * 0.05f;
|
||||
for (int i = 0; i < 21; i++) {
|
||||
xrt_vec3 pos = hand->kps[i];
|
||||
float len = m_vec3_len_sqrd(pos); // Faster.
|
||||
if (len > max_dist_from_camera_sqrd) {
|
||||
return false;
|
||||
U_LOG_W("Hand is somewhere we wouldn't expect!");
|
||||
}
|
||||
if (len < min_dist_from_camera_sqrd) {
|
||||
return false;
|
||||
}
|
||||
if (pos.z > 0.0f) { // remember negative-Z is forward!
|
||||
return false;
|
||||
}
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
static bool
|
||||
rejectBadHand(Hand3D *hand)
|
||||
{
|
||||
if (!rejectTooFarOrTooClose(hand)) {
|
||||
return false;
|
||||
}
|
||||
// todo: add lots of checks! finger length, fingers bending backwards, etc.
|
||||
return true;
|
||||
}
|
||||
208
src/xrt/drivers/ht/ht_image_math.hpp
Normal file
208
src/xrt/drivers/ht/ht_image_math.hpp
Normal file
|
|
@ -0,0 +1,208 @@
|
|||
// Copyright 2021, Collabora, Ltd.
|
||||
// SPDX-License-Identifier: BSL-1.0
|
||||
/*!
|
||||
* @file
|
||||
* @brief Helper math to do things with images for the camera-based hand tracker
|
||||
* @author Moses Turner <moses@collabora.com>
|
||||
* @ingroup drv_ht
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
#include "ht_driver.hpp"
|
||||
#include "math/m_api.h"
|
||||
#include "math/m_vec2.h"
|
||||
#include "math/m_vec3.h"
|
||||
#include "xrt/xrt_defines.h"
|
||||
#include <opencv2/calib3d.hpp>
|
||||
|
||||
#include <opencv2/core/types.hpp>
|
||||
#include <opencv2/imgproc.hpp>
|
||||
|
||||
static cv::Scalar
|
||||
hsv2rgb(float fH, float fS, float fV)
|
||||
{
|
||||
float fC = fV * fS; // Chroma
|
||||
float fHPrime = fmod(fH / 60.0, 6);
|
||||
float fX = fC * (1 - fabs(fmod(fHPrime, 2) - 1));
|
||||
float fM = fV - fC;
|
||||
|
||||
float fR, fG, fB;
|
||||
|
||||
if (0 <= fHPrime && fHPrime < 1) {
|
||||
fR = fC;
|
||||
fG = fX;
|
||||
fB = 0;
|
||||
} else if (1 <= fHPrime && fHPrime < 2) {
|
||||
fR = fX;
|
||||
fG = fC;
|
||||
fB = 0;
|
||||
} else if (2 <= fHPrime && fHPrime < 3) {
|
||||
fR = 0;
|
||||
fG = fC;
|
||||
fB = fX;
|
||||
} else if (3 <= fHPrime && fHPrime < 4) {
|
||||
fR = 0;
|
||||
fG = fX;
|
||||
fB = fC;
|
||||
} else if (4 <= fHPrime && fHPrime < 5) {
|
||||
fR = fX;
|
||||
fG = 0;
|
||||
fB = fC;
|
||||
} else if (5 <= fHPrime && fHPrime < 6) {
|
||||
fR = fC;
|
||||
fG = 0;
|
||||
fB = fX;
|
||||
} else {
|
||||
fR = 0;
|
||||
fG = 0;
|
||||
fB = 0;
|
||||
}
|
||||
|
||||
fR += fM;
|
||||
fG += fM;
|
||||
fB += fM;
|
||||
return {fR * 255.0f, fG * 255.0f, fB * 255.0f};
|
||||
}
|
||||
|
||||
static xrt_vec3
|
||||
raycoord(struct ht_view *htv, struct xrt_vec3 model_out)
|
||||
{
|
||||
cv::Mat in_px_coords(1, 1, CV_32FC2);
|
||||
float *write_in;
|
||||
write_in = in_px_coords.ptr<float>(0);
|
||||
write_in[0] = model_out.x;
|
||||
write_in[1] = model_out.y;
|
||||
cv::Mat out_ray(1, 1, CV_32FC2);
|
||||
|
||||
cv::fisheye::undistortPoints(in_px_coords, out_ray, htv->cameraMatrix, htv->distortion);
|
||||
|
||||
|
||||
float n_x = out_ray.at<float>(0, 0);
|
||||
float n_y = out_ray.at<float>(0, 1);
|
||||
|
||||
|
||||
struct xrt_vec3 n = {n_x, n_y, 1.0f};
|
||||
|
||||
cv::Matx33f R = htv->rotate_camera_to_stereo_camera;
|
||||
|
||||
struct xrt_vec3 o = {
|
||||
(n.x * R(0, 0)) + (n.y * R(0, 1)) + (n.z * R(0, 2)),
|
||||
(n.x * R(1, 0)) + (n.y * R(1, 1)) + (n.z * R(1, 2)),
|
||||
(n.x * R(2, 0)) + (n.y * R(2, 1)) + (n.z * R(2, 2)),
|
||||
};
|
||||
|
||||
math_vec3_scalar_mul(1.0f / o.z, &o);
|
||||
return o;
|
||||
}
|
||||
|
||||
|
||||
/*!
|
||||
* Returns a 2x3 transform matrix that takes you back from the blackbarred image to the original image.
|
||||
*/
|
||||
|
||||
static cv::Matx23f
|
||||
blackbar(cv::Mat &in, cv::Mat &out, xrt_vec2_i32 out_size)
|
||||
{
|
||||
float aspect_ratio_input = in.cols / in.rows;
|
||||
float aspect_ratio_output = out_size.x / out_size.y;
|
||||
|
||||
if (aspect_ratio_input == aspect_ratio_output) {
|
||||
cv::resize(in, out, {out_size.x, out_size.y});
|
||||
cv::Matx23f ret;
|
||||
float scale_from_out_to_in = (float)in.cols / (float)out_size.x;
|
||||
// clang-format off
|
||||
ret(0,0) = scale_from_out_to_in; ret(0,1) = 0.0f; ret(0,2) = 0.0f;
|
||||
ret(1,0) = 0.0f; ret(1,1) = scale_from_out_to_in; ret(1,2) = 0.0f;
|
||||
// clang-format on
|
||||
return ret;
|
||||
}
|
||||
assert(!"Uh oh! Unimplemented!");
|
||||
return {};
|
||||
}
|
||||
|
||||
/*!
|
||||
* This is a template so that we can use xrt_vec3 or xrt_vec2.
|
||||
* Please don't use this for anything other than xrt_vec3 or xrt_vec2!
|
||||
*/
|
||||
|
||||
template <typename T>
|
||||
T
|
||||
transformVecBy2x3(T in, cv::Matx23f warp_back)
|
||||
{
|
||||
T rr;
|
||||
rr.x = (in.x * warp_back(0, 0)) + (in.y * warp_back(0, 1)) + warp_back(0, 2);
|
||||
rr.y = (in.x * warp_back(1, 0)) + (in.y * warp_back(1, 1)) + warp_back(1, 2);
|
||||
return rr;
|
||||
}
|
||||
|
||||
//! Draw some dots. Factors out some boilerplate.
|
||||
static void
|
||||
handDot(cv::Mat &mat, xrt_vec2 place, float radius, float hue, int type)
|
||||
{
|
||||
cv::circle(mat, {(int)place.x, (int)place.y}, radius, hsv2rgb(hue * 360.0f, 1.0f, 1.0f), type);
|
||||
}
|
||||
|
||||
static DetectionModelOutput
|
||||
rotatedRectFromJoints(struct ht_view *htv, xrt_vec2 middle, xrt_vec2 wrist, DetectionModelOutput *out)
|
||||
{
|
||||
// Close to what Mediapipe does, but slightly different - just uses the middle proximal instead of "estimating"
|
||||
// it from the pinky and index.
|
||||
// at the end of the day I should probably do that basis vector filtering thing to get a nicer middle metacarpal
|
||||
// from 6 keypoints (not thumb proximal) OR SHOULD I. because distortion. hmm
|
||||
|
||||
// Feel free to look at the way MP does it, you can see it's different.
|
||||
// https://github.com/google/mediapipe/blob/master/mediapipe/modules/holistic_landmark/calculators/hand_detections_from_pose_to_rects_calculator.cc
|
||||
|
||||
struct xrt_vec2 hand_center = middle; // Middle proximal, straight-up.
|
||||
|
||||
struct xrt_vec2 wrist_to_middle = middle - wrist;
|
||||
|
||||
float box_size = m_vec2_len(wrist_to_middle) * 2.0f * 1.7f;
|
||||
|
||||
double rot = atan2(wrist_to_middle.x, wrist_to_middle.y) * (-180.0f / M_PI);
|
||||
|
||||
out->rotation = rot;
|
||||
out->size = box_size;
|
||||
out->center = hand_center;
|
||||
|
||||
cv::RotatedRect rrect =
|
||||
cv::RotatedRect(cv::Point2f(out->center.x, out->center.y), cv::Size2f(out->size, out->size), out->rotation);
|
||||
|
||||
|
||||
cv::Point2f vertices[4];
|
||||
rrect.points(vertices);
|
||||
if (htv->htd->debug_scribble) {
|
||||
for (int i = 0; i < 4; i++)
|
||||
line(htv->debug_out_to_this, vertices[i], vertices[(i + 1) % 4], cv::Scalar(i * 63, i * 63, 0),
|
||||
2);
|
||||
}
|
||||
// topright is 0. bottomright is 1. bottomleft is 2. topleft is 3.
|
||||
|
||||
cv::Point2f src_tri[3] = {vertices[3], vertices[2], vertices[1]}; // top-left, bottom-left, bottom-right
|
||||
|
||||
cv::Point2f dest_tri[3] = {cv::Point2f(0, 0), cv::Point2f(0, 224), cv::Point2f(224, 224)};
|
||||
|
||||
out->warp_there = getAffineTransform(src_tri, dest_tri);
|
||||
out->warp_back = getAffineTransform(dest_tri, src_tri);
|
||||
|
||||
out->wrist = wrist;
|
||||
|
||||
return *out;
|
||||
}
|
||||
|
||||
static void
|
||||
planarize(cv::Mat &input, uint8_t *output)
|
||||
{
|
||||
// output better be the right size, because we are not doing any bounds checking!
|
||||
assert(input.isContinuous());
|
||||
int lix = input.cols;
|
||||
int liy = input.rows;
|
||||
cv::Mat planes[3];
|
||||
cv::split(input, planes);
|
||||
cv::Mat red = planes[0];
|
||||
cv::Mat green = planes[1];
|
||||
cv::Mat blue = planes[2];
|
||||
memcpy(output, red.data, lix * liy);
|
||||
memcpy(output + (lix * liy), green.data, lix * liy);
|
||||
memcpy(output + (lix * liy * 2), blue.data, lix * liy);
|
||||
}
|
||||
|
|
@ -1,16 +1,19 @@
|
|||
// Copyright 2020, Collabora, Ltd.
|
||||
// Copyright 2020-2021, Collabora, Ltd.
|
||||
// SPDX-License-Identifier: BSL-1.0
|
||||
/*!
|
||||
* @file
|
||||
* @brief Interface to camera based hand tracking driver code.
|
||||
* @author Christoph Haag <christoph.haag@collabora.com>
|
||||
* @author Moses Turner <moses@collabora.com>
|
||||
* @ingroup drv_ht
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "math/m_api.h"
|
||||
#include "xrt/xrt_defines.h"
|
||||
#include "xrt/xrt_device.h"
|
||||
#include "vive/vive_config.h"
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
|
|
@ -24,12 +27,12 @@ extern "C" {
|
|||
*/
|
||||
|
||||
/*!
|
||||
* Create a probe for camera based hand tracking.
|
||||
* Create a hand tracker device.
|
||||
*
|
||||
* @ingroup drv_ht
|
||||
*/
|
||||
struct xrt_auto_prober *
|
||||
ht_create_auto_prober();
|
||||
struct xrt_device *
|
||||
ht_device_create(struct xrt_prober *xp, struct t_stereo_camera_calibration *calib);
|
||||
|
||||
/*!
|
||||
* @dir drivers/ht
|
||||
|
|
|
|||
819
src/xrt/drivers/ht/ht_models.hpp
Normal file
819
src/xrt/drivers/ht/ht_models.hpp
Normal file
|
|
@ -0,0 +1,819 @@
|
|||
// Copyright 2021, Collabora, Ltd.
|
||||
// SPDX-License-Identifier: BSL-1.0
|
||||
/*!
|
||||
* @file
|
||||
* @brief Code to run machine learning models for camera-based hand tracker.
|
||||
* @author Moses Turner <moses@collabora.com>
|
||||
* @author Marcus Edel <marcus.edel@collabora.com>
|
||||
* @ingroup drv_ht
|
||||
*/
|
||||
|
||||
// Many C api things were stolen from here (MIT license):
|
||||
// https://github.com/microsoft/onnxruntime-inference-examples/blob/main/c_cxx/fns_candy_style_transfer/fns_candy_style_transfer.c
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "math/m_api.h"
|
||||
#include "math/m_vec2.h"
|
||||
#include "math/m_vec3.h"
|
||||
#include "opencv2/calib3d.hpp"
|
||||
#include "opencv2/highgui.hpp"
|
||||
#include "os/os_threading.h"
|
||||
#include "os/os_time.h"
|
||||
#include "util/u_json.h"
|
||||
|
||||
|
||||
#include "../depthai/depthai_interface.h"
|
||||
#include "../vf/vf_interface.h"
|
||||
|
||||
#include <cjson/cJSON.h>
|
||||
|
||||
#include "ht_driver.hpp"
|
||||
#include "ht_nms.hpp"
|
||||
#include "util/u_logging.h"
|
||||
#include "util/u_time.h"
|
||||
#include "util/u_trace_marker.h"
|
||||
#include "ht_image_math.hpp"
|
||||
|
||||
#include <core/session/onnxruntime_c_api.h>
|
||||
#include <cstdlib>
|
||||
#include <opencv2/core/types.hpp>
|
||||
#include <opencv2/imgproc.hpp>
|
||||
#include <string.h>
|
||||
#include <stdio.h>
|
||||
#include <math.h>
|
||||
|
||||
#include <exception>
|
||||
#include <fstream>
|
||||
#include <iostream>
|
||||
#include <limits>
|
||||
#include <numeric>
|
||||
#include <cmath>
|
||||
#include <sstream>
|
||||
#include <vector>
|
||||
|
||||
#define ORT_CHECK(g_ort, expr) \
|
||||
do { \
|
||||
OrtStatus *onnx_status = (expr); \
|
||||
if (onnx_status != nullptr) { \
|
||||
const char *msg = g_ort->GetErrorMessage(onnx_status); \
|
||||
U_LOG_E("at %s:%d: %s\n", __FILE__, __LINE__, msg); \
|
||||
g_ort->ReleaseStatus(onnx_status); \
|
||||
assert(false); \
|
||||
} \
|
||||
} while (0);
|
||||
|
||||
static std::vector<std::vector<float>> anchor{
|
||||
{0.031250, 0.031250, 1.000000, 1.000000}, {0.031250, 0.031250, 1.000000, 1.000000},
|
||||
{0.093750, 0.031250, 1.000000, 1.000000}, {0.093750, 0.031250, 1.000000, 1.000000},
|
||||
{0.156250, 0.031250, 1.000000, 1.000000}, {0.156250, 0.031250, 1.000000, 1.000000},
|
||||
{0.218750, 0.031250, 1.000000, 1.000000}, {0.218750, 0.031250, 1.000000, 1.000000},
|
||||
{0.281250, 0.031250, 1.000000, 1.000000}, {0.281250, 0.031250, 1.000000, 1.000000},
|
||||
{0.343750, 0.031250, 1.000000, 1.000000}, {0.343750, 0.031250, 1.000000, 1.000000},
|
||||
{0.406250, 0.031250, 1.000000, 1.000000}, {0.406250, 0.031250, 1.000000, 1.000000},
|
||||
{0.468750, 0.031250, 1.000000, 1.000000}, {0.468750, 0.031250, 1.000000, 1.000000},
|
||||
{0.531250, 0.031250, 1.000000, 1.000000}, {0.531250, 0.031250, 1.000000, 1.000000},
|
||||
{0.593750, 0.031250, 1.000000, 1.000000}, {0.593750, 0.031250, 1.000000, 1.000000},
|
||||
{0.656250, 0.031250, 1.000000, 1.000000}, {0.656250, 0.031250, 1.000000, 1.000000},
|
||||
{0.718750, 0.031250, 1.000000, 1.000000}, {0.718750, 0.031250, 1.000000, 1.000000},
|
||||
{0.781250, 0.031250, 1.000000, 1.000000}, {0.781250, 0.031250, 1.000000, 1.000000},
|
||||
{0.843750, 0.031250, 1.000000, 1.000000}, {0.843750, 0.031250, 1.000000, 1.000000},
|
||||
{0.906250, 0.031250, 1.000000, 1.000000}, {0.906250, 0.031250, 1.000000, 1.000000},
|
||||
{0.968750, 0.031250, 1.000000, 1.000000}, {0.968750, 0.031250, 1.000000, 1.000000},
|
||||
{0.031250, 0.093750, 1.000000, 1.000000}, {0.031250, 0.093750, 1.000000, 1.000000},
|
||||
{0.093750, 0.093750, 1.000000, 1.000000}, {0.093750, 0.093750, 1.000000, 1.000000},
|
||||
{0.156250, 0.093750, 1.000000, 1.000000}, {0.156250, 0.093750, 1.000000, 1.000000},
|
||||
{0.218750, 0.093750, 1.000000, 1.000000}, {0.218750, 0.093750, 1.000000, 1.000000},
|
||||
{0.281250, 0.093750, 1.000000, 1.000000}, {0.281250, 0.093750, 1.000000, 1.000000},
|
||||
{0.343750, 0.093750, 1.000000, 1.000000}, {0.343750, 0.093750, 1.000000, 1.000000},
|
||||
{0.406250, 0.093750, 1.000000, 1.000000}, {0.406250, 0.093750, 1.000000, 1.000000},
|
||||
{0.468750, 0.093750, 1.000000, 1.000000}, {0.468750, 0.093750, 1.000000, 1.000000},
|
||||
{0.531250, 0.093750, 1.000000, 1.000000}, {0.531250, 0.093750, 1.000000, 1.000000},
|
||||
{0.593750, 0.093750, 1.000000, 1.000000}, {0.593750, 0.093750, 1.000000, 1.000000},
|
||||
{0.656250, 0.093750, 1.000000, 1.000000}, {0.656250, 0.093750, 1.000000, 1.000000},
|
||||
{0.718750, 0.093750, 1.000000, 1.000000}, {0.718750, 0.093750, 1.000000, 1.000000},
|
||||
{0.781250, 0.093750, 1.000000, 1.000000}, {0.781250, 0.093750, 1.000000, 1.000000},
|
||||
{0.843750, 0.093750, 1.000000, 1.000000}, {0.843750, 0.093750, 1.000000, 1.000000},
|
||||
{0.906250, 0.093750, 1.000000, 1.000000}, {0.906250, 0.093750, 1.000000, 1.000000},
|
||||
{0.968750, 0.093750, 1.000000, 1.000000}, {0.968750, 0.093750, 1.000000, 1.000000},
|
||||
{0.031250, 0.156250, 1.000000, 1.000000}, {0.031250, 0.156250, 1.000000, 1.000000},
|
||||
{0.093750, 0.156250, 1.000000, 1.000000}, {0.093750, 0.156250, 1.000000, 1.000000},
|
||||
{0.156250, 0.156250, 1.000000, 1.000000}, {0.156250, 0.156250, 1.000000, 1.000000},
|
||||
{0.218750, 0.156250, 1.000000, 1.000000}, {0.218750, 0.156250, 1.000000, 1.000000},
|
||||
{0.281250, 0.156250, 1.000000, 1.000000}, {0.281250, 0.156250, 1.000000, 1.000000},
|
||||
{0.343750, 0.156250, 1.000000, 1.000000}, {0.343750, 0.156250, 1.000000, 1.000000},
|
||||
{0.406250, 0.156250, 1.000000, 1.000000}, {0.406250, 0.156250, 1.000000, 1.000000},
|
||||
{0.468750, 0.156250, 1.000000, 1.000000}, {0.468750, 0.156250, 1.000000, 1.000000},
|
||||
{0.531250, 0.156250, 1.000000, 1.000000}, {0.531250, 0.156250, 1.000000, 1.000000},
|
||||
{0.593750, 0.156250, 1.000000, 1.000000}, {0.593750, 0.156250, 1.000000, 1.000000},
|
||||
{0.656250, 0.156250, 1.000000, 1.000000}, {0.656250, 0.156250, 1.000000, 1.000000},
|
||||
{0.718750, 0.156250, 1.000000, 1.000000}, {0.718750, 0.156250, 1.000000, 1.000000},
|
||||
{0.781250, 0.156250, 1.000000, 1.000000}, {0.781250, 0.156250, 1.000000, 1.000000},
|
||||
{0.843750, 0.156250, 1.000000, 1.000000}, {0.843750, 0.156250, 1.000000, 1.000000},
|
||||
{0.906250, 0.156250, 1.000000, 1.000000}, {0.906250, 0.156250, 1.000000, 1.000000},
|
||||
{0.968750, 0.156250, 1.000000, 1.000000}, {0.968750, 0.156250, 1.000000, 1.000000},
|
||||
{0.031250, 0.218750, 1.000000, 1.000000}, {0.031250, 0.218750, 1.000000, 1.000000},
|
||||
{0.093750, 0.218750, 1.000000, 1.000000}, {0.093750, 0.218750, 1.000000, 1.000000},
|
||||
{0.156250, 0.218750, 1.000000, 1.000000}, {0.156250, 0.218750, 1.000000, 1.000000},
|
||||
{0.218750, 0.218750, 1.000000, 1.000000}, {0.218750, 0.218750, 1.000000, 1.000000},
|
||||
{0.281250, 0.218750, 1.000000, 1.000000}, {0.281250, 0.218750, 1.000000, 1.000000},
|
||||
{0.343750, 0.218750, 1.000000, 1.000000}, {0.343750, 0.218750, 1.000000, 1.000000},
|
||||
{0.406250, 0.218750, 1.000000, 1.000000}, {0.406250, 0.218750, 1.000000, 1.000000},
|
||||
{0.468750, 0.218750, 1.000000, 1.000000}, {0.468750, 0.218750, 1.000000, 1.000000},
|
||||
{0.531250, 0.218750, 1.000000, 1.000000}, {0.531250, 0.218750, 1.000000, 1.000000},
|
||||
{0.593750, 0.218750, 1.000000, 1.000000}, {0.593750, 0.218750, 1.000000, 1.000000},
|
||||
{0.656250, 0.218750, 1.000000, 1.000000}, {0.656250, 0.218750, 1.000000, 1.000000},
|
||||
{0.718750, 0.218750, 1.000000, 1.000000}, {0.718750, 0.218750, 1.000000, 1.000000},
|
||||
{0.781250, 0.218750, 1.000000, 1.000000}, {0.781250, 0.218750, 1.000000, 1.000000},
|
||||
{0.843750, 0.218750, 1.000000, 1.000000}, {0.843750, 0.218750, 1.000000, 1.000000},
|
||||
{0.906250, 0.218750, 1.000000, 1.000000}, {0.906250, 0.218750, 1.000000, 1.000000},
|
||||
{0.968750, 0.218750, 1.000000, 1.000000}, {0.968750, 0.218750, 1.000000, 1.000000},
|
||||
{0.031250, 0.281250, 1.000000, 1.000000}, {0.031250, 0.281250, 1.000000, 1.000000},
|
||||
{0.093750, 0.281250, 1.000000, 1.000000}, {0.093750, 0.281250, 1.000000, 1.000000},
|
||||
{0.156250, 0.281250, 1.000000, 1.000000}, {0.156250, 0.281250, 1.000000, 1.000000},
|
||||
{0.218750, 0.281250, 1.000000, 1.000000}, {0.218750, 0.281250, 1.000000, 1.000000},
|
||||
{0.281250, 0.281250, 1.000000, 1.000000}, {0.281250, 0.281250, 1.000000, 1.000000},
|
||||
{0.343750, 0.281250, 1.000000, 1.000000}, {0.343750, 0.281250, 1.000000, 1.000000},
|
||||
{0.406250, 0.281250, 1.000000, 1.000000}, {0.406250, 0.281250, 1.000000, 1.000000},
|
||||
{0.468750, 0.281250, 1.000000, 1.000000}, {0.468750, 0.281250, 1.000000, 1.000000},
|
||||
{0.531250, 0.281250, 1.000000, 1.000000}, {0.531250, 0.281250, 1.000000, 1.000000},
|
||||
{0.593750, 0.281250, 1.000000, 1.000000}, {0.593750, 0.281250, 1.000000, 1.000000},
|
||||
{0.656250, 0.281250, 1.000000, 1.000000}, {0.656250, 0.281250, 1.000000, 1.000000},
|
||||
{0.718750, 0.281250, 1.000000, 1.000000}, {0.718750, 0.281250, 1.000000, 1.000000},
|
||||
{0.781250, 0.281250, 1.000000, 1.000000}, {0.781250, 0.281250, 1.000000, 1.000000},
|
||||
{0.843750, 0.281250, 1.000000, 1.000000}, {0.843750, 0.281250, 1.000000, 1.000000},
|
||||
{0.906250, 0.281250, 1.000000, 1.000000}, {0.906250, 0.281250, 1.000000, 1.000000},
|
||||
{0.968750, 0.281250, 1.000000, 1.000000}, {0.968750, 0.281250, 1.000000, 1.000000},
|
||||
{0.031250, 0.343750, 1.000000, 1.000000}, {0.031250, 0.343750, 1.000000, 1.000000},
|
||||
{0.093750, 0.343750, 1.000000, 1.000000}, {0.093750, 0.343750, 1.000000, 1.000000},
|
||||
{0.156250, 0.343750, 1.000000, 1.000000}, {0.156250, 0.343750, 1.000000, 1.000000},
|
||||
{0.218750, 0.343750, 1.000000, 1.000000}, {0.218750, 0.343750, 1.000000, 1.000000},
|
||||
{0.281250, 0.343750, 1.000000, 1.000000}, {0.281250, 0.343750, 1.000000, 1.000000},
|
||||
{0.343750, 0.343750, 1.000000, 1.000000}, {0.343750, 0.343750, 1.000000, 1.000000},
|
||||
{0.406250, 0.343750, 1.000000, 1.000000}, {0.406250, 0.343750, 1.000000, 1.000000},
|
||||
{0.468750, 0.343750, 1.000000, 1.000000}, {0.468750, 0.343750, 1.000000, 1.000000},
|
||||
{0.531250, 0.343750, 1.000000, 1.000000}, {0.531250, 0.343750, 1.000000, 1.000000},
|
||||
{0.593750, 0.343750, 1.000000, 1.000000}, {0.593750, 0.343750, 1.000000, 1.000000},
|
||||
{0.656250, 0.343750, 1.000000, 1.000000}, {0.656250, 0.343750, 1.000000, 1.000000},
|
||||
{0.718750, 0.343750, 1.000000, 1.000000}, {0.718750, 0.343750, 1.000000, 1.000000},
|
||||
{0.781250, 0.343750, 1.000000, 1.000000}, {0.781250, 0.343750, 1.000000, 1.000000},
|
||||
{0.843750, 0.343750, 1.000000, 1.000000}, {0.843750, 0.343750, 1.000000, 1.000000},
|
||||
{0.906250, 0.343750, 1.000000, 1.000000}, {0.906250, 0.343750, 1.000000, 1.000000},
|
||||
{0.968750, 0.343750, 1.000000, 1.000000}, {0.968750, 0.343750, 1.000000, 1.000000},
|
||||
{0.031250, 0.406250, 1.000000, 1.000000}, {0.031250, 0.406250, 1.000000, 1.000000},
|
||||
{0.093750, 0.406250, 1.000000, 1.000000}, {0.093750, 0.406250, 1.000000, 1.000000},
|
||||
{0.156250, 0.406250, 1.000000, 1.000000}, {0.156250, 0.406250, 1.000000, 1.000000},
|
||||
{0.218750, 0.406250, 1.000000, 1.000000}, {0.218750, 0.406250, 1.000000, 1.000000},
|
||||
{0.281250, 0.406250, 1.000000, 1.000000}, {0.281250, 0.406250, 1.000000, 1.000000},
|
||||
{0.343750, 0.406250, 1.000000, 1.000000}, {0.343750, 0.406250, 1.000000, 1.000000},
|
||||
{0.406250, 0.406250, 1.000000, 1.000000}, {0.406250, 0.406250, 1.000000, 1.000000},
|
||||
{0.468750, 0.406250, 1.000000, 1.000000}, {0.468750, 0.406250, 1.000000, 1.000000},
|
||||
{0.531250, 0.406250, 1.000000, 1.000000}, {0.531250, 0.406250, 1.000000, 1.000000},
|
||||
{0.593750, 0.406250, 1.000000, 1.000000}, {0.593750, 0.406250, 1.000000, 1.000000},
|
||||
{0.656250, 0.406250, 1.000000, 1.000000}, {0.656250, 0.406250, 1.000000, 1.000000},
|
||||
{0.718750, 0.406250, 1.000000, 1.000000}, {0.718750, 0.406250, 1.000000, 1.000000},
|
||||
{0.781250, 0.406250, 1.000000, 1.000000}, {0.781250, 0.406250, 1.000000, 1.000000},
|
||||
{0.843750, 0.406250, 1.000000, 1.000000}, {0.843750, 0.406250, 1.000000, 1.000000},
|
||||
{0.906250, 0.406250, 1.000000, 1.000000}, {0.906250, 0.406250, 1.000000, 1.000000},
|
||||
{0.968750, 0.406250, 1.000000, 1.000000}, {0.968750, 0.406250, 1.000000, 1.000000},
|
||||
{0.031250, 0.468750, 1.000000, 1.000000}, {0.031250, 0.468750, 1.000000, 1.000000},
|
||||
{0.093750, 0.468750, 1.000000, 1.000000}, {0.093750, 0.468750, 1.000000, 1.000000},
|
||||
{0.156250, 0.468750, 1.000000, 1.000000}, {0.156250, 0.468750, 1.000000, 1.000000},
|
||||
{0.218750, 0.468750, 1.000000, 1.000000}, {0.218750, 0.468750, 1.000000, 1.000000},
|
||||
{0.281250, 0.468750, 1.000000, 1.000000}, {0.281250, 0.468750, 1.000000, 1.000000},
|
||||
{0.343750, 0.468750, 1.000000, 1.000000}, {0.343750, 0.468750, 1.000000, 1.000000},
|
||||
{0.406250, 0.468750, 1.000000, 1.000000}, {0.406250, 0.468750, 1.000000, 1.000000},
|
||||
{0.468750, 0.468750, 1.000000, 1.000000}, {0.468750, 0.468750, 1.000000, 1.000000},
|
||||
{0.531250, 0.468750, 1.000000, 1.000000}, {0.531250, 0.468750, 1.000000, 1.000000},
|
||||
{0.593750, 0.468750, 1.000000, 1.000000}, {0.593750, 0.468750, 1.000000, 1.000000},
|
||||
{0.656250, 0.468750, 1.000000, 1.000000}, {0.656250, 0.468750, 1.000000, 1.000000},
|
||||
{0.718750, 0.468750, 1.000000, 1.000000}, {0.718750, 0.468750, 1.000000, 1.000000},
|
||||
{0.781250, 0.468750, 1.000000, 1.000000}, {0.781250, 0.468750, 1.000000, 1.000000},
|
||||
{0.843750, 0.468750, 1.000000, 1.000000}, {0.843750, 0.468750, 1.000000, 1.000000},
|
||||
{0.906250, 0.468750, 1.000000, 1.000000}, {0.906250, 0.468750, 1.000000, 1.000000},
|
||||
{0.968750, 0.468750, 1.000000, 1.000000}, {0.968750, 0.468750, 1.000000, 1.000000},
|
||||
{0.031250, 0.531250, 1.000000, 1.000000}, {0.031250, 0.531250, 1.000000, 1.000000},
|
||||
{0.093750, 0.531250, 1.000000, 1.000000}, {0.093750, 0.531250, 1.000000, 1.000000},
|
||||
{0.156250, 0.531250, 1.000000, 1.000000}, {0.156250, 0.531250, 1.000000, 1.000000},
|
||||
{0.218750, 0.531250, 1.000000, 1.000000}, {0.218750, 0.531250, 1.000000, 1.000000},
|
||||
{0.281250, 0.531250, 1.000000, 1.000000}, {0.281250, 0.531250, 1.000000, 1.000000},
|
||||
{0.343750, 0.531250, 1.000000, 1.000000}, {0.343750, 0.531250, 1.000000, 1.000000},
|
||||
{0.406250, 0.531250, 1.000000, 1.000000}, {0.406250, 0.531250, 1.000000, 1.000000},
|
||||
{0.468750, 0.531250, 1.000000, 1.000000}, {0.468750, 0.531250, 1.000000, 1.000000},
|
||||
{0.531250, 0.531250, 1.000000, 1.000000}, {0.531250, 0.531250, 1.000000, 1.000000},
|
||||
{0.593750, 0.531250, 1.000000, 1.000000}, {0.593750, 0.531250, 1.000000, 1.000000},
|
||||
{0.656250, 0.531250, 1.000000, 1.000000}, {0.656250, 0.531250, 1.000000, 1.000000},
|
||||
{0.718750, 0.531250, 1.000000, 1.000000}, {0.718750, 0.531250, 1.000000, 1.000000},
|
||||
{0.781250, 0.531250, 1.000000, 1.000000}, {0.781250, 0.531250, 1.000000, 1.000000},
|
||||
{0.843750, 0.531250, 1.000000, 1.000000}, {0.843750, 0.531250, 1.000000, 1.000000},
|
||||
{0.906250, 0.531250, 1.000000, 1.000000}, {0.906250, 0.531250, 1.000000, 1.000000},
|
||||
{0.968750, 0.531250, 1.000000, 1.000000}, {0.968750, 0.531250, 1.000000, 1.000000},
|
||||
{0.031250, 0.593750, 1.000000, 1.000000}, {0.031250, 0.593750, 1.000000, 1.000000},
|
||||
{0.093750, 0.593750, 1.000000, 1.000000}, {0.093750, 0.593750, 1.000000, 1.000000},
|
||||
{0.156250, 0.593750, 1.000000, 1.000000}, {0.156250, 0.593750, 1.000000, 1.000000},
|
||||
{0.218750, 0.593750, 1.000000, 1.000000}, {0.218750, 0.593750, 1.000000, 1.000000},
|
||||
{0.281250, 0.593750, 1.000000, 1.000000}, {0.281250, 0.593750, 1.000000, 1.000000},
|
||||
{0.343750, 0.593750, 1.000000, 1.000000}, {0.343750, 0.593750, 1.000000, 1.000000},
|
||||
{0.406250, 0.593750, 1.000000, 1.000000}, {0.406250, 0.593750, 1.000000, 1.000000},
|
||||
{0.468750, 0.593750, 1.000000, 1.000000}, {0.468750, 0.593750, 1.000000, 1.000000},
|
||||
{0.531250, 0.593750, 1.000000, 1.000000}, {0.531250, 0.593750, 1.000000, 1.000000},
|
||||
{0.593750, 0.593750, 1.000000, 1.000000}, {0.593750, 0.593750, 1.000000, 1.000000},
|
||||
{0.656250, 0.593750, 1.000000, 1.000000}, {0.656250, 0.593750, 1.000000, 1.000000},
|
||||
{0.718750, 0.593750, 1.000000, 1.000000}, {0.718750, 0.593750, 1.000000, 1.000000},
|
||||
{0.781250, 0.593750, 1.000000, 1.000000}, {0.781250, 0.593750, 1.000000, 1.000000},
|
||||
{0.843750, 0.593750, 1.000000, 1.000000}, {0.843750, 0.593750, 1.000000, 1.000000},
|
||||
{0.906250, 0.593750, 1.000000, 1.000000}, {0.906250, 0.593750, 1.000000, 1.000000},
|
||||
{0.968750, 0.593750, 1.000000, 1.000000}, {0.968750, 0.593750, 1.000000, 1.000000},
|
||||
{0.031250, 0.656250, 1.000000, 1.000000}, {0.031250, 0.656250, 1.000000, 1.000000},
|
||||
{0.093750, 0.656250, 1.000000, 1.000000}, {0.093750, 0.656250, 1.000000, 1.000000},
|
||||
{0.156250, 0.656250, 1.000000, 1.000000}, {0.156250, 0.656250, 1.000000, 1.000000},
|
||||
{0.218750, 0.656250, 1.000000, 1.000000}, {0.218750, 0.656250, 1.000000, 1.000000},
|
||||
{0.281250, 0.656250, 1.000000, 1.000000}, {0.281250, 0.656250, 1.000000, 1.000000},
|
||||
{0.343750, 0.656250, 1.000000, 1.000000}, {0.343750, 0.656250, 1.000000, 1.000000},
|
||||
{0.406250, 0.656250, 1.000000, 1.000000}, {0.406250, 0.656250, 1.000000, 1.000000},
|
||||
{0.468750, 0.656250, 1.000000, 1.000000}, {0.468750, 0.656250, 1.000000, 1.000000},
|
||||
{0.531250, 0.656250, 1.000000, 1.000000}, {0.531250, 0.656250, 1.000000, 1.000000},
|
||||
{0.593750, 0.656250, 1.000000, 1.000000}, {0.593750, 0.656250, 1.000000, 1.000000},
|
||||
{0.656250, 0.656250, 1.000000, 1.000000}, {0.656250, 0.656250, 1.000000, 1.000000},
|
||||
{0.718750, 0.656250, 1.000000, 1.000000}, {0.718750, 0.656250, 1.000000, 1.000000},
|
||||
{0.781250, 0.656250, 1.000000, 1.000000}, {0.781250, 0.656250, 1.000000, 1.000000},
|
||||
{0.843750, 0.656250, 1.000000, 1.000000}, {0.843750, 0.656250, 1.000000, 1.000000},
|
||||
{0.906250, 0.656250, 1.000000, 1.000000}, {0.906250, 0.656250, 1.000000, 1.000000},
|
||||
{0.968750, 0.656250, 1.000000, 1.000000}, {0.968750, 0.656250, 1.000000, 1.000000},
|
||||
{0.031250, 0.718750, 1.000000, 1.000000}, {0.031250, 0.718750, 1.000000, 1.000000},
|
||||
{0.093750, 0.718750, 1.000000, 1.000000}, {0.093750, 0.718750, 1.000000, 1.000000},
|
||||
{0.156250, 0.718750, 1.000000, 1.000000}, {0.156250, 0.718750, 1.000000, 1.000000},
|
||||
{0.218750, 0.718750, 1.000000, 1.000000}, {0.218750, 0.718750, 1.000000, 1.000000},
|
||||
{0.281250, 0.718750, 1.000000, 1.000000}, {0.281250, 0.718750, 1.000000, 1.000000},
|
||||
{0.343750, 0.718750, 1.000000, 1.000000}, {0.343750, 0.718750, 1.000000, 1.000000},
|
||||
{0.406250, 0.718750, 1.000000, 1.000000}, {0.406250, 0.718750, 1.000000, 1.000000},
|
||||
{0.468750, 0.718750, 1.000000, 1.000000}, {0.468750, 0.718750, 1.000000, 1.000000},
|
||||
{0.531250, 0.718750, 1.000000, 1.000000}, {0.531250, 0.718750, 1.000000, 1.000000},
|
||||
{0.593750, 0.718750, 1.000000, 1.000000}, {0.593750, 0.718750, 1.000000, 1.000000},
|
||||
{0.656250, 0.718750, 1.000000, 1.000000}, {0.656250, 0.718750, 1.000000, 1.000000},
|
||||
{0.718750, 0.718750, 1.000000, 1.000000}, {0.718750, 0.718750, 1.000000, 1.000000},
|
||||
{0.781250, 0.718750, 1.000000, 1.000000}, {0.781250, 0.718750, 1.000000, 1.000000},
|
||||
{0.843750, 0.718750, 1.000000, 1.000000}, {0.843750, 0.718750, 1.000000, 1.000000},
|
||||
{0.906250, 0.718750, 1.000000, 1.000000}, {0.906250, 0.718750, 1.000000, 1.000000},
|
||||
{0.968750, 0.718750, 1.000000, 1.000000}, {0.968750, 0.718750, 1.000000, 1.000000},
|
||||
{0.031250, 0.781250, 1.000000, 1.000000}, {0.031250, 0.781250, 1.000000, 1.000000},
|
||||
{0.093750, 0.781250, 1.000000, 1.000000}, {0.093750, 0.781250, 1.000000, 1.000000},
|
||||
{0.156250, 0.781250, 1.000000, 1.000000}, {0.156250, 0.781250, 1.000000, 1.000000},
|
||||
{0.218750, 0.781250, 1.000000, 1.000000}, {0.218750, 0.781250, 1.000000, 1.000000},
|
||||
{0.281250, 0.781250, 1.000000, 1.000000}, {0.281250, 0.781250, 1.000000, 1.000000},
|
||||
{0.343750, 0.781250, 1.000000, 1.000000}, {0.343750, 0.781250, 1.000000, 1.000000},
|
||||
{0.406250, 0.781250, 1.000000, 1.000000}, {0.406250, 0.781250, 1.000000, 1.000000},
|
||||
{0.468750, 0.781250, 1.000000, 1.000000}, {0.468750, 0.781250, 1.000000, 1.000000},
|
||||
{0.531250, 0.781250, 1.000000, 1.000000}, {0.531250, 0.781250, 1.000000, 1.000000},
|
||||
{0.593750, 0.781250, 1.000000, 1.000000}, {0.593750, 0.781250, 1.000000, 1.000000},
|
||||
{0.656250, 0.781250, 1.000000, 1.000000}, {0.656250, 0.781250, 1.000000, 1.000000},
|
||||
{0.718750, 0.781250, 1.000000, 1.000000}, {0.718750, 0.781250, 1.000000, 1.000000},
|
||||
{0.781250, 0.781250, 1.000000, 1.000000}, {0.781250, 0.781250, 1.000000, 1.000000},
|
||||
{0.843750, 0.781250, 1.000000, 1.000000}, {0.843750, 0.781250, 1.000000, 1.000000},
|
||||
{0.906250, 0.781250, 1.000000, 1.000000}, {0.906250, 0.781250, 1.000000, 1.000000},
|
||||
{0.968750, 0.781250, 1.000000, 1.000000}, {0.968750, 0.781250, 1.000000, 1.000000},
|
||||
{0.031250, 0.843750, 1.000000, 1.000000}, {0.031250, 0.843750, 1.000000, 1.000000},
|
||||
{0.093750, 0.843750, 1.000000, 1.000000}, {0.093750, 0.843750, 1.000000, 1.000000},
|
||||
{0.156250, 0.843750, 1.000000, 1.000000}, {0.156250, 0.843750, 1.000000, 1.000000},
|
||||
{0.218750, 0.843750, 1.000000, 1.000000}, {0.218750, 0.843750, 1.000000, 1.000000},
|
||||
{0.281250, 0.843750, 1.000000, 1.000000}, {0.281250, 0.843750, 1.000000, 1.000000},
|
||||
{0.343750, 0.843750, 1.000000, 1.000000}, {0.343750, 0.843750, 1.000000, 1.000000},
|
||||
{0.406250, 0.843750, 1.000000, 1.000000}, {0.406250, 0.843750, 1.000000, 1.000000},
|
||||
{0.468750, 0.843750, 1.000000, 1.000000}, {0.468750, 0.843750, 1.000000, 1.000000},
|
||||
{0.531250, 0.843750, 1.000000, 1.000000}, {0.531250, 0.843750, 1.000000, 1.000000},
|
||||
{0.593750, 0.843750, 1.000000, 1.000000}, {0.593750, 0.843750, 1.000000, 1.000000},
|
||||
{0.656250, 0.843750, 1.000000, 1.000000}, {0.656250, 0.843750, 1.000000, 1.000000},
|
||||
{0.718750, 0.843750, 1.000000, 1.000000}, {0.718750, 0.843750, 1.000000, 1.000000},
|
||||
{0.781250, 0.843750, 1.000000, 1.000000}, {0.781250, 0.843750, 1.000000, 1.000000},
|
||||
{0.843750, 0.843750, 1.000000, 1.000000}, {0.843750, 0.843750, 1.000000, 1.000000},
|
||||
{0.906250, 0.843750, 1.000000, 1.000000}, {0.906250, 0.843750, 1.000000, 1.000000},
|
||||
{0.968750, 0.843750, 1.000000, 1.000000}, {0.968750, 0.843750, 1.000000, 1.000000},
|
||||
{0.031250, 0.906250, 1.000000, 1.000000}, {0.031250, 0.906250, 1.000000, 1.000000},
|
||||
{0.093750, 0.906250, 1.000000, 1.000000}, {0.093750, 0.906250, 1.000000, 1.000000},
|
||||
{0.156250, 0.906250, 1.000000, 1.000000}, {0.156250, 0.906250, 1.000000, 1.000000},
|
||||
{0.218750, 0.906250, 1.000000, 1.000000}, {0.218750, 0.906250, 1.000000, 1.000000},
|
||||
{0.281250, 0.906250, 1.000000, 1.000000}, {0.281250, 0.906250, 1.000000, 1.000000},
|
||||
{0.343750, 0.906250, 1.000000, 1.000000}, {0.343750, 0.906250, 1.000000, 1.000000},
|
||||
{0.406250, 0.906250, 1.000000, 1.000000}, {0.406250, 0.906250, 1.000000, 1.000000},
|
||||
{0.468750, 0.906250, 1.000000, 1.000000}, {0.468750, 0.906250, 1.000000, 1.000000},
|
||||
{0.531250, 0.906250, 1.000000, 1.000000}, {0.531250, 0.906250, 1.000000, 1.000000},
|
||||
{0.593750, 0.906250, 1.000000, 1.000000}, {0.593750, 0.906250, 1.000000, 1.000000},
|
||||
{0.656250, 0.906250, 1.000000, 1.000000}, {0.656250, 0.906250, 1.000000, 1.000000},
|
||||
{0.718750, 0.906250, 1.000000, 1.000000}, {0.718750, 0.906250, 1.000000, 1.000000},
|
||||
{0.781250, 0.906250, 1.000000, 1.000000}, {0.781250, 0.906250, 1.000000, 1.000000},
|
||||
{0.843750, 0.906250, 1.000000, 1.000000}, {0.843750, 0.906250, 1.000000, 1.000000},
|
||||
{0.906250, 0.906250, 1.000000, 1.000000}, {0.906250, 0.906250, 1.000000, 1.000000},
|
||||
{0.968750, 0.906250, 1.000000, 1.000000}, {0.968750, 0.906250, 1.000000, 1.000000},
|
||||
{0.031250, 0.968750, 1.000000, 1.000000}, {0.031250, 0.968750, 1.000000, 1.000000},
|
||||
{0.093750, 0.968750, 1.000000, 1.000000}, {0.093750, 0.968750, 1.000000, 1.000000},
|
||||
{0.156250, 0.968750, 1.000000, 1.000000}, {0.156250, 0.968750, 1.000000, 1.000000},
|
||||
{0.218750, 0.968750, 1.000000, 1.000000}, {0.218750, 0.968750, 1.000000, 1.000000},
|
||||
{0.281250, 0.968750, 1.000000, 1.000000}, {0.281250, 0.968750, 1.000000, 1.000000},
|
||||
{0.343750, 0.968750, 1.000000, 1.000000}, {0.343750, 0.968750, 1.000000, 1.000000},
|
||||
{0.406250, 0.968750, 1.000000, 1.000000}, {0.406250, 0.968750, 1.000000, 1.000000},
|
||||
{0.468750, 0.968750, 1.000000, 1.000000}, {0.468750, 0.968750, 1.000000, 1.000000},
|
||||
{0.531250, 0.968750, 1.000000, 1.000000}, {0.531250, 0.968750, 1.000000, 1.000000},
|
||||
{0.593750, 0.968750, 1.000000, 1.000000}, {0.593750, 0.968750, 1.000000, 1.000000},
|
||||
{0.656250, 0.968750, 1.000000, 1.000000}, {0.656250, 0.968750, 1.000000, 1.000000},
|
||||
{0.718750, 0.968750, 1.000000, 1.000000}, {0.718750, 0.968750, 1.000000, 1.000000},
|
||||
{0.781250, 0.968750, 1.000000, 1.000000}, {0.781250, 0.968750, 1.000000, 1.000000},
|
||||
{0.843750, 0.968750, 1.000000, 1.000000}, {0.843750, 0.968750, 1.000000, 1.000000},
|
||||
{0.906250, 0.968750, 1.000000, 1.000000}, {0.906250, 0.968750, 1.000000, 1.000000},
|
||||
{0.968750, 0.968750, 1.000000, 1.000000}, {0.968750, 0.968750, 1.000000, 1.000000},
|
||||
{0.062500, 0.062500, 1.000000, 1.000000}, {0.062500, 0.062500, 1.000000, 1.000000},
|
||||
{0.187500, 0.062500, 1.000000, 1.000000}, {0.187500, 0.062500, 1.000000, 1.000000},
|
||||
{0.312500, 0.062500, 1.000000, 1.000000}, {0.312500, 0.062500, 1.000000, 1.000000},
|
||||
{0.437500, 0.062500, 1.000000, 1.000000}, {0.437500, 0.062500, 1.000000, 1.000000},
|
||||
{0.562500, 0.062500, 1.000000, 1.000000}, {0.562500, 0.062500, 1.000000, 1.000000},
|
||||
{0.687500, 0.062500, 1.000000, 1.000000}, {0.687500, 0.062500, 1.000000, 1.000000},
|
||||
{0.812500, 0.062500, 1.000000, 1.000000}, {0.812500, 0.062500, 1.000000, 1.000000},
|
||||
{0.937500, 0.062500, 1.000000, 1.000000}, {0.937500, 0.062500, 1.000000, 1.000000},
|
||||
{0.062500, 0.187500, 1.000000, 1.000000}, {0.062500, 0.187500, 1.000000, 1.000000},
|
||||
{0.187500, 0.187500, 1.000000, 1.000000}, {0.187500, 0.187500, 1.000000, 1.000000},
|
||||
{0.312500, 0.187500, 1.000000, 1.000000}, {0.312500, 0.187500, 1.000000, 1.000000},
|
||||
{0.437500, 0.187500, 1.000000, 1.000000}, {0.437500, 0.187500, 1.000000, 1.000000},
|
||||
{0.562500, 0.187500, 1.000000, 1.000000}, {0.562500, 0.187500, 1.000000, 1.000000},
|
||||
{0.687500, 0.187500, 1.000000, 1.000000}, {0.687500, 0.187500, 1.000000, 1.000000},
|
||||
{0.812500, 0.187500, 1.000000, 1.000000}, {0.812500, 0.187500, 1.000000, 1.000000},
|
||||
{0.937500, 0.187500, 1.000000, 1.000000}, {0.937500, 0.187500, 1.000000, 1.000000},
|
||||
{0.062500, 0.312500, 1.000000, 1.000000}, {0.062500, 0.312500, 1.000000, 1.000000},
|
||||
{0.187500, 0.312500, 1.000000, 1.000000}, {0.187500, 0.312500, 1.000000, 1.000000},
|
||||
{0.312500, 0.312500, 1.000000, 1.000000}, {0.312500, 0.312500, 1.000000, 1.000000},
|
||||
{0.437500, 0.312500, 1.000000, 1.000000}, {0.437500, 0.312500, 1.000000, 1.000000},
|
||||
{0.562500, 0.312500, 1.000000, 1.000000}, {0.562500, 0.312500, 1.000000, 1.000000},
|
||||
{0.687500, 0.312500, 1.000000, 1.000000}, {0.687500, 0.312500, 1.000000, 1.000000},
|
||||
{0.812500, 0.312500, 1.000000, 1.000000}, {0.812500, 0.312500, 1.000000, 1.000000},
|
||||
{0.937500, 0.312500, 1.000000, 1.000000}, {0.937500, 0.312500, 1.000000, 1.000000},
|
||||
{0.062500, 0.437500, 1.000000, 1.000000}, {0.062500, 0.437500, 1.000000, 1.000000},
|
||||
{0.187500, 0.437500, 1.000000, 1.000000}, {0.187500, 0.437500, 1.000000, 1.000000},
|
||||
{0.312500, 0.437500, 1.000000, 1.000000}, {0.312500, 0.437500, 1.000000, 1.000000},
|
||||
{0.437500, 0.437500, 1.000000, 1.000000}, {0.437500, 0.437500, 1.000000, 1.000000},
|
||||
{0.562500, 0.437500, 1.000000, 1.000000}, {0.562500, 0.437500, 1.000000, 1.000000},
|
||||
{0.687500, 0.437500, 1.000000, 1.000000}, {0.687500, 0.437500, 1.000000, 1.000000},
|
||||
{0.812500, 0.437500, 1.000000, 1.000000}, {0.812500, 0.437500, 1.000000, 1.000000},
|
||||
{0.937500, 0.437500, 1.000000, 1.000000}, {0.937500, 0.437500, 1.000000, 1.000000},
|
||||
{0.062500, 0.562500, 1.000000, 1.000000}, {0.062500, 0.562500, 1.000000, 1.000000},
|
||||
{0.187500, 0.562500, 1.000000, 1.000000}, {0.187500, 0.562500, 1.000000, 1.000000},
|
||||
{0.312500, 0.562500, 1.000000, 1.000000}, {0.312500, 0.562500, 1.000000, 1.000000},
|
||||
{0.437500, 0.562500, 1.000000, 1.000000}, {0.437500, 0.562500, 1.000000, 1.000000},
|
||||
{0.562500, 0.562500, 1.000000, 1.000000}, {0.562500, 0.562500, 1.000000, 1.000000},
|
||||
{0.687500, 0.562500, 1.000000, 1.000000}, {0.687500, 0.562500, 1.000000, 1.000000},
|
||||
{0.812500, 0.562500, 1.000000, 1.000000}, {0.812500, 0.562500, 1.000000, 1.000000},
|
||||
{0.937500, 0.562500, 1.000000, 1.000000}, {0.937500, 0.562500, 1.000000, 1.000000},
|
||||
{0.062500, 0.687500, 1.000000, 1.000000}, {0.062500, 0.687500, 1.000000, 1.000000},
|
||||
{0.187500, 0.687500, 1.000000, 1.000000}, {0.187500, 0.687500, 1.000000, 1.000000},
|
||||
{0.312500, 0.687500, 1.000000, 1.000000}, {0.312500, 0.687500, 1.000000, 1.000000},
|
||||
{0.437500, 0.687500, 1.000000, 1.000000}, {0.437500, 0.687500, 1.000000, 1.000000},
|
||||
{0.562500, 0.687500, 1.000000, 1.000000}, {0.562500, 0.687500, 1.000000, 1.000000},
|
||||
{0.687500, 0.687500, 1.000000, 1.000000}, {0.687500, 0.687500, 1.000000, 1.000000},
|
||||
{0.812500, 0.687500, 1.000000, 1.000000}, {0.812500, 0.687500, 1.000000, 1.000000},
|
||||
{0.937500, 0.687500, 1.000000, 1.000000}, {0.937500, 0.687500, 1.000000, 1.000000},
|
||||
{0.062500, 0.812500, 1.000000, 1.000000}, {0.062500, 0.812500, 1.000000, 1.000000},
|
||||
{0.187500, 0.812500, 1.000000, 1.000000}, {0.187500, 0.812500, 1.000000, 1.000000},
|
||||
{0.312500, 0.812500, 1.000000, 1.000000}, {0.312500, 0.812500, 1.000000, 1.000000},
|
||||
{0.437500, 0.812500, 1.000000, 1.000000}, {0.437500, 0.812500, 1.000000, 1.000000},
|
||||
{0.562500, 0.812500, 1.000000, 1.000000}, {0.562500, 0.812500, 1.000000, 1.000000},
|
||||
{0.687500, 0.812500, 1.000000, 1.000000}, {0.687500, 0.812500, 1.000000, 1.000000},
|
||||
{0.812500, 0.812500, 1.000000, 1.000000}, {0.812500, 0.812500, 1.000000, 1.000000},
|
||||
{0.937500, 0.812500, 1.000000, 1.000000}, {0.937500, 0.812500, 1.000000, 1.000000},
|
||||
{0.062500, 0.937500, 1.000000, 1.000000}, {0.062500, 0.937500, 1.000000, 1.000000},
|
||||
{0.187500, 0.937500, 1.000000, 1.000000}, {0.187500, 0.937500, 1.000000, 1.000000},
|
||||
{0.312500, 0.937500, 1.000000, 1.000000}, {0.312500, 0.937500, 1.000000, 1.000000},
|
||||
{0.437500, 0.937500, 1.000000, 1.000000}, {0.437500, 0.937500, 1.000000, 1.000000},
|
||||
{0.562500, 0.937500, 1.000000, 1.000000}, {0.562500, 0.937500, 1.000000, 1.000000},
|
||||
{0.687500, 0.937500, 1.000000, 1.000000}, {0.687500, 0.937500, 1.000000, 1.000000},
|
||||
{0.812500, 0.937500, 1.000000, 1.000000}, {0.812500, 0.937500, 1.000000, 1.000000},
|
||||
{0.937500, 0.937500, 1.000000, 1.000000}, {0.937500, 0.937500, 1.000000, 1.000000},
|
||||
{0.125000, 0.125000, 1.000000, 1.000000}, {0.125000, 0.125000, 1.000000, 1.000000},
|
||||
{0.125000, 0.125000, 1.000000, 1.000000}, {0.125000, 0.125000, 1.000000, 1.000000},
|
||||
{0.125000, 0.125000, 1.000000, 1.000000}, {0.125000, 0.125000, 1.000000, 1.000000},
|
||||
{0.375000, 0.125000, 1.000000, 1.000000}, {0.375000, 0.125000, 1.000000, 1.000000},
|
||||
{0.375000, 0.125000, 1.000000, 1.000000}, {0.375000, 0.125000, 1.000000, 1.000000},
|
||||
{0.375000, 0.125000, 1.000000, 1.000000}, {0.375000, 0.125000, 1.000000, 1.000000},
|
||||
{0.625000, 0.125000, 1.000000, 1.000000}, {0.625000, 0.125000, 1.000000, 1.000000},
|
||||
{0.625000, 0.125000, 1.000000, 1.000000}, {0.625000, 0.125000, 1.000000, 1.000000},
|
||||
{0.625000, 0.125000, 1.000000, 1.000000}, {0.625000, 0.125000, 1.000000, 1.000000},
|
||||
{0.875000, 0.125000, 1.000000, 1.000000}, {0.875000, 0.125000, 1.000000, 1.000000},
|
||||
{0.875000, 0.125000, 1.000000, 1.000000}, {0.875000, 0.125000, 1.000000, 1.000000},
|
||||
{0.875000, 0.125000, 1.000000, 1.000000}, {0.875000, 0.125000, 1.000000, 1.000000},
|
||||
{0.125000, 0.375000, 1.000000, 1.000000}, {0.125000, 0.375000, 1.000000, 1.000000},
|
||||
{0.125000, 0.375000, 1.000000, 1.000000}, {0.125000, 0.375000, 1.000000, 1.000000},
|
||||
{0.125000, 0.375000, 1.000000, 1.000000}, {0.125000, 0.375000, 1.000000, 1.000000},
|
||||
{0.375000, 0.375000, 1.000000, 1.000000}, {0.375000, 0.375000, 1.000000, 1.000000},
|
||||
{0.375000, 0.375000, 1.000000, 1.000000}, {0.375000, 0.375000, 1.000000, 1.000000},
|
||||
{0.375000, 0.375000, 1.000000, 1.000000}, {0.375000, 0.375000, 1.000000, 1.000000},
|
||||
{0.625000, 0.375000, 1.000000, 1.000000}, {0.625000, 0.375000, 1.000000, 1.000000},
|
||||
{0.625000, 0.375000, 1.000000, 1.000000}, {0.625000, 0.375000, 1.000000, 1.000000},
|
||||
{0.625000, 0.375000, 1.000000, 1.000000}, {0.625000, 0.375000, 1.000000, 1.000000},
|
||||
{0.875000, 0.375000, 1.000000, 1.000000}, {0.875000, 0.375000, 1.000000, 1.000000},
|
||||
{0.875000, 0.375000, 1.000000, 1.000000}, {0.875000, 0.375000, 1.000000, 1.000000},
|
||||
{0.875000, 0.375000, 1.000000, 1.000000}, {0.875000, 0.375000, 1.000000, 1.000000},
|
||||
{0.125000, 0.625000, 1.000000, 1.000000}, {0.125000, 0.625000, 1.000000, 1.000000},
|
||||
{0.125000, 0.625000, 1.000000, 1.000000}, {0.125000, 0.625000, 1.000000, 1.000000},
|
||||
{0.125000, 0.625000, 1.000000, 1.000000}, {0.125000, 0.625000, 1.000000, 1.000000},
|
||||
{0.375000, 0.625000, 1.000000, 1.000000}, {0.375000, 0.625000, 1.000000, 1.000000},
|
||||
{0.375000, 0.625000, 1.000000, 1.000000}, {0.375000, 0.625000, 1.000000, 1.000000},
|
||||
{0.375000, 0.625000, 1.000000, 1.000000}, {0.375000, 0.625000, 1.000000, 1.000000},
|
||||
{0.625000, 0.625000, 1.000000, 1.000000}, {0.625000, 0.625000, 1.000000, 1.000000},
|
||||
{0.625000, 0.625000, 1.000000, 1.000000}, {0.625000, 0.625000, 1.000000, 1.000000},
|
||||
{0.625000, 0.625000, 1.000000, 1.000000}, {0.625000, 0.625000, 1.000000, 1.000000},
|
||||
{0.875000, 0.625000, 1.000000, 1.000000}, {0.875000, 0.625000, 1.000000, 1.000000},
|
||||
{0.875000, 0.625000, 1.000000, 1.000000}, {0.875000, 0.625000, 1.000000, 1.000000},
|
||||
{0.875000, 0.625000, 1.000000, 1.000000}, {0.875000, 0.625000, 1.000000, 1.000000},
|
||||
{0.125000, 0.875000, 1.000000, 1.000000}, {0.125000, 0.875000, 1.000000, 1.000000},
|
||||
{0.125000, 0.875000, 1.000000, 1.000000}, {0.125000, 0.875000, 1.000000, 1.000000},
|
||||
{0.125000, 0.875000, 1.000000, 1.000000}, {0.125000, 0.875000, 1.000000, 1.000000},
|
||||
{0.375000, 0.875000, 1.000000, 1.000000}, {0.375000, 0.875000, 1.000000, 1.000000},
|
||||
{0.375000, 0.875000, 1.000000, 1.000000}, {0.375000, 0.875000, 1.000000, 1.000000},
|
||||
{0.375000, 0.875000, 1.000000, 1.000000}, {0.375000, 0.875000, 1.000000, 1.000000},
|
||||
{0.625000, 0.875000, 1.000000, 1.000000}, {0.625000, 0.875000, 1.000000, 1.000000},
|
||||
{0.625000, 0.875000, 1.000000, 1.000000}, {0.625000, 0.875000, 1.000000, 1.000000},
|
||||
{0.625000, 0.875000, 1.000000, 1.000000}, {0.625000, 0.875000, 1.000000, 1.000000},
|
||||
{0.875000, 0.875000, 1.000000, 1.000000}, {0.875000, 0.875000, 1.000000, 1.000000},
|
||||
{0.875000, 0.875000, 1.000000, 1.000000}, {0.875000, 0.875000, 1.000000, 1.000000},
|
||||
{0.875000, 0.875000, 1.000000, 1.000000}, {0.875000, 0.875000, 1.000000, 1.000000}};
|
||||
|
||||
static Hand2D
|
||||
runKeypointEstimator(struct ht_view *htv, cv::Mat img)
|
||||
{
|
||||
const size_t lix = 224;
|
||||
const size_t liy = 224;
|
||||
cv::Mat planes[3];
|
||||
|
||||
|
||||
uint8_t combined_planes[224 * 224 * 3];
|
||||
planarize(img, combined_planes);
|
||||
|
||||
// Normalize - supposedly, the keypoint estimator wants keypoints in [0,1]
|
||||
float real_thing[lix * liy * 3] = {0};
|
||||
for (size_t i = 0; i < lix * liy * 3; i++) {
|
||||
real_thing[i] = (float)combined_planes[i] / 255.0;
|
||||
}
|
||||
|
||||
const OrtApi *g_ort = htv->htd->ort_api;
|
||||
struct ModelInfo *model = &htv->keypoint_model;
|
||||
|
||||
|
||||
OrtValue *input_tensor = nullptr;
|
||||
|
||||
|
||||
ORT_CHECK(g_ort, g_ort->CreateTensorWithDataAsOrtValue(model->memoryInfo, real_thing, model->input_size_bytes,
|
||||
model->input_shape.data(), model->input_shape.size(),
|
||||
ONNX_TENSOR_ELEMENT_DATA_TYPE_FLOAT, &input_tensor));
|
||||
|
||||
// Cargo-culted
|
||||
assert(input_tensor != nullptr);
|
||||
int is_tensor;
|
||||
ORT_CHECK(g_ort, g_ort->IsTensor(input_tensor, &is_tensor));
|
||||
assert(is_tensor);
|
||||
|
||||
const char *output_names[] = {"Identity", "Identity_2", "Identity_2"};
|
||||
|
||||
// If any of these are non-NULL, ONNX will explode and won't tell you why! Be extremely paranoid!
|
||||
OrtValue *output_tensor[3] = {nullptr, nullptr, nullptr};
|
||||
ORT_CHECK(g_ort, g_ort->Run(model->session, nullptr, model->input_names.data(), &input_tensor, 1, output_names,
|
||||
3, output_tensor));
|
||||
|
||||
ORT_CHECK(g_ort, g_ort->IsTensor(output_tensor[0], &is_tensor));
|
||||
assert(is_tensor);
|
||||
|
||||
float *landmarks = nullptr;
|
||||
|
||||
// Should give a pointer to data that is freed on g_ort->ReleaseValue(output_tensor[0]);.
|
||||
ORT_CHECK(g_ort, g_ort->GetTensorMutableData(output_tensor[0], (void **)&landmarks));
|
||||
|
||||
int stride = 3;
|
||||
Hand2D dumb;
|
||||
for (size_t i = 0; i < 21; i++) {
|
||||
int rt = i * stride;
|
||||
float x = landmarks[rt];
|
||||
float y = landmarks[rt + 1];
|
||||
float z = landmarks[rt + 2];
|
||||
dumb.kps[i].x = x;
|
||||
dumb.kps[i].y = y;
|
||||
dumb.kps[i].z = z;
|
||||
}
|
||||
|
||||
// We have to get to here, or else we leak a whole lot! If you need to return early, make this into a goto!
|
||||
g_ort->ReleaseValue(output_tensor[0]);
|
||||
g_ort->ReleaseValue(output_tensor[1]);
|
||||
g_ort->ReleaseValue(output_tensor[2]);
|
||||
g_ort->ReleaseValue(input_tensor);
|
||||
return dumb;
|
||||
}
|
||||
|
||||
|
||||
|
||||
static std::vector<Palm7KP>
|
||||
runHandDetector(struct ht_view *htv, cv::Mat &raw_input)
|
||||
{
|
||||
const OrtApi *g_ort = htv->htd->ort_api;
|
||||
|
||||
cv::Mat img;
|
||||
|
||||
const int hd_size = 128;
|
||||
const int inputTensorSize = hd_size * hd_size * 3 * sizeof(float);
|
||||
|
||||
cv::Matx23f back_from_blackbar = blackbar(raw_input, img, {hd_size, hd_size});
|
||||
|
||||
float scale_factor = back_from_blackbar(0, 0); // 960/128
|
||||
assert(img.isContinuous());
|
||||
float mean = 128.0f;
|
||||
float std = 128.0f;
|
||||
float real_thing[hd_size * hd_size * 3];
|
||||
|
||||
|
||||
if (htv->htd->runtime_config.palm_detection_use_mediapipe) {
|
||||
uint8_t combined_planes[hd_size * hd_size * 3] = {0};
|
||||
planarize(img, combined_planes);
|
||||
for (size_t i = 0; i < hd_size * hd_size * 3; i++) {
|
||||
float val = (float)combined_planes[i];
|
||||
real_thing[i] = (val - mean) / std;
|
||||
}
|
||||
// Hope it was worth it...
|
||||
} else {
|
||||
|
||||
assert(img.isContinuous());
|
||||
|
||||
for (size_t i = 0; i < hd_size * hd_size * 3; i++) {
|
||||
int val = img.data[i];
|
||||
|
||||
real_thing[i] = (val - mean) / std;
|
||||
}
|
||||
}
|
||||
|
||||
// Convenience
|
||||
struct ModelInfo *model_hd = &htv->detection_model;
|
||||
|
||||
// If this is non-NULL, ONNX will explode and won't tell you why!
|
||||
OrtValue *input_tensor = nullptr;
|
||||
|
||||
ORT_CHECK(g_ort, g_ort->CreateTensorWithDataAsOrtValue(
|
||||
model_hd->memoryInfo, real_thing, inputTensorSize, model_hd->input_shape.data(),
|
||||
model_hd->input_shape.size(), ONNX_TENSOR_ELEMENT_DATA_TYPE_FLOAT, &input_tensor));
|
||||
|
||||
// Cargo-culted
|
||||
assert(input_tensor != nullptr);
|
||||
int is_tensor;
|
||||
ORT_CHECK(g_ort, g_ort->IsTensor(input_tensor, &is_tensor));
|
||||
assert(is_tensor);
|
||||
|
||||
// If any of these are non-NULL, ONNX will explode and won't tell you why! Be extremely paranoid!
|
||||
OrtValue *output_tensor[2] = {nullptr, nullptr};
|
||||
ORT_CHECK(g_ort, g_ort->Run(model_hd->session, nullptr, model_hd->input_names.data(),
|
||||
(const OrtValue *const *)&input_tensor, model_hd->input_names.size(),
|
||||
model_hd->output_names.data(), model_hd->output_names.size(), output_tensor));
|
||||
|
||||
ORT_CHECK(g_ort, g_ort->IsTensor(output_tensor[0], &is_tensor));
|
||||
assert(is_tensor);
|
||||
|
||||
float *classificators = nullptr;
|
||||
float *regressors = nullptr;
|
||||
|
||||
ORT_CHECK(g_ort,
|
||||
g_ort->GetTensorMutableData(output_tensor[0], (void **)&classificators)); // Totally won't segfault!
|
||||
ORT_CHECK(g_ort, g_ort->GetTensorMutableData(output_tensor[1], (void **)®ressors));
|
||||
// We need to free these!
|
||||
|
||||
float *rg = regressors; // shorter name
|
||||
|
||||
std::vector<NMSPalm> detections;
|
||||
int count = 0;
|
||||
size_t i = 0;
|
||||
|
||||
std::vector<Palm7KP> output;
|
||||
std::vector<NMSPalm> nms_palms;
|
||||
|
||||
|
||||
for (std::vector<std::vector<float>>::iterator it = anchor.begin(); it != anchor.end(); ++it, ++i) {
|
||||
std::vector<float>::iterator anchorData = it->begin();
|
||||
|
||||
float score0 = classificators[i];
|
||||
float score = 1.0 / (1.0 + exp(-score0));
|
||||
|
||||
if (score > 0.6) {
|
||||
// Boundary box.
|
||||
NMSPalm det;
|
||||
|
||||
float anchx = *(anchorData + 0) * 128;
|
||||
float anchy = *(anchorData + 1) * 128;
|
||||
|
||||
float shiftx = regressors[i * 18];
|
||||
float shifty = regressors[i * 18 + 1];
|
||||
|
||||
float w = regressors[i * 18 + 2];
|
||||
float h = regressors[i * 18 + 3];
|
||||
|
||||
|
||||
|
||||
float cx = shiftx + anchx;
|
||||
float cy = shifty + anchy;
|
||||
|
||||
struct xrt_vec2 *kps = det.keypoints;
|
||||
|
||||
kps[0] = {rg[i * 18 + 4], rg[i * 18 + 5]};
|
||||
kps[1] = {rg[i * 18 + 6], rg[i * 18 + 7]};
|
||||
kps[2] = {rg[i * 18 + 8], rg[i * 18 + 9]};
|
||||
kps[3] = {rg[i * 18 + 10], rg[i * 18 + 11]};
|
||||
kps[4] = {rg[i * 18 + 12], rg[i * 18 + 13]};
|
||||
kps[5] = {rg[i * 18 + 14], rg[i * 18 + 15]};
|
||||
kps[6] = {rg[i * 18 + 16], rg[i * 18 + 17]};
|
||||
|
||||
|
||||
for (int i = 0; i < 7; i++) {
|
||||
struct xrt_vec2 *b = &kps[i];
|
||||
b->x += anchx;
|
||||
b->y += anchy;
|
||||
}
|
||||
|
||||
det.bbox.w = w;
|
||||
det.bbox.h = h;
|
||||
det.bbox.cx = cx;
|
||||
det.bbox.cy = cy;
|
||||
det.confidence = score;
|
||||
detections.push_back(det);
|
||||
count++;
|
||||
|
||||
|
||||
|
||||
int square = fmax(w, h);
|
||||
|
||||
square = square / scale_factor;
|
||||
}
|
||||
}
|
||||
|
||||
if (count == 0) {
|
||||
goto cleanup;
|
||||
}
|
||||
|
||||
nms_palms = filterBoxesWeightedAvg(detections);
|
||||
|
||||
|
||||
|
||||
for (NMSPalm cooler : nms_palms) {
|
||||
|
||||
// Display box
|
||||
struct xrt_vec2 tl = {cooler.bbox.cx - cooler.bbox.w / 2, cooler.bbox.cy - cooler.bbox.h / 2};
|
||||
struct xrt_vec2 bob = transformVecBy2x3(tl, back_from_blackbar);
|
||||
float sz = cooler.bbox.w / scale_factor;
|
||||
|
||||
if (htv->htd->debug_scribble) {
|
||||
cv::rectangle(htv->debug_out_to_this, {(int)bob.x, (int)bob.y, (int)sz, (int)sz}, {0, 0, 255},
|
||||
5);
|
||||
}
|
||||
|
||||
|
||||
Palm7KP this_element;
|
||||
|
||||
for (int i = 0; i < 7; i++) {
|
||||
struct xrt_vec2 b = cooler.keypoints[i];
|
||||
this_element.kps[i] = transformVecBy2x3(b, back_from_blackbar);
|
||||
if (htv->htd->debug_scribble) {
|
||||
handDot(htv->debug_out_to_this, this_element.kps[i], 5, ((float)i) * (360.0f / 7.0f),
|
||||
2);
|
||||
}
|
||||
}
|
||||
|
||||
output.push_back(this_element);
|
||||
}
|
||||
|
||||
cleanup:
|
||||
g_ort->ReleaseValue(output_tensor[0]);
|
||||
g_ort->ReleaseValue(output_tensor[1]);
|
||||
g_ort->ReleaseValue(input_tensor);
|
||||
return output;
|
||||
}
|
||||
|
||||
|
||||
static void
|
||||
addSlug(struct ht_device *htd, const char *suffix, char *out)
|
||||
{
|
||||
strcpy(out, htd->runtime_config.model_slug);
|
||||
strcat(out, suffix);
|
||||
}
|
||||
|
||||
static void
|
||||
initKeypointEstimator(struct ht_device *htd, ht_view *htv)
|
||||
{
|
||||
struct ModelInfo *model_ke = &htv->keypoint_model;
|
||||
const OrtApi *g_ort = htd->ort_api;
|
||||
OrtSessionOptions *opts = nullptr;
|
||||
|
||||
ORT_CHECK(g_ort, g_ort->CreateSessionOptions(&opts));
|
||||
|
||||
ORT_CHECK(g_ort, g_ort->SetSessionGraphOptimizationLevel(opts, ORT_ENABLE_ALL));
|
||||
ORT_CHECK(g_ort, g_ort->SetIntraOpNumThreads(opts, 1));
|
||||
|
||||
char modelLocation[1024];
|
||||
if (htd->runtime_config.keypoint_estimation_use_mediapipe) {
|
||||
addSlug(htd, "hand_landmark_MEDIAPIPE.onnx", modelLocation);
|
||||
} else {
|
||||
addSlug(htd, "hand_landmark_COLLABORA.onnx", modelLocation);
|
||||
}
|
||||
ORT_CHECK(g_ort, g_ort->CreateSession(htd->ort_env, modelLocation, opts, &model_ke->session));
|
||||
g_ort->ReleaseSessionOptions(opts);
|
||||
|
||||
model_ke->input_shape.push_back(1);
|
||||
model_ke->input_shape.push_back(3);
|
||||
model_ke->input_shape.push_back(224);
|
||||
model_ke->input_shape.push_back(224);
|
||||
|
||||
model_ke->input_names.push_back("input_1");
|
||||
|
||||
model_ke->output_names.push_back("Identity");
|
||||
model_ke->output_names.push_back("Identity_1");
|
||||
model_ke->output_names.push_back("Identity_2");
|
||||
|
||||
model_ke->input_size_bytes = 224 * 224 * 3 * sizeof(float);
|
||||
|
||||
ORT_CHECK(g_ort, g_ort->CreateCpuMemoryInfo(OrtArenaAllocator, OrtMemTypeDefault, &model_ke->memoryInfo));
|
||||
|
||||
htv->run_keypoint_model = runKeypointEstimator;
|
||||
}
|
||||
|
||||
static void
|
||||
initHandDetector(struct ht_device *htd, ht_view *htv)
|
||||
{
|
||||
struct ModelInfo *model_hd = &htv->detection_model;
|
||||
memset(model_hd, 0, sizeof(struct ModelInfo));
|
||||
|
||||
const OrtApi *g_ort = htd->ort_api;
|
||||
OrtSessionOptions *opts = nullptr;
|
||||
ORT_CHECK(g_ort, g_ort->CreateSessionOptions(&opts));
|
||||
|
||||
ORT_CHECK(g_ort, g_ort->SetSessionGraphOptimizationLevel(opts, ORT_ENABLE_ALL));
|
||||
ORT_CHECK(g_ort, g_ort->SetIntraOpNumThreads(opts, 1));
|
||||
|
||||
char modelLocation[1024];
|
||||
|
||||
// Hard-coded. Even though you can use the ONNX runtime's API to dynamically figure these out, that doesn't make
|
||||
// any sense because these don't change between runs, and if you are swapping models you have to do much more
|
||||
// than just change the input/output names.
|
||||
if (htd->runtime_config.palm_detection_use_mediapipe) {
|
||||
addSlug(htd, "palm_detection_MEDIAPIPE.onnx", modelLocation);
|
||||
model_hd->input_shape.push_back(1);
|
||||
model_hd->input_shape.push_back(3);
|
||||
model_hd->input_shape.push_back(128);
|
||||
model_hd->input_shape.push_back(128);
|
||||
|
||||
model_hd->input_names.push_back("input");
|
||||
} else {
|
||||
addSlug(htd, "palm_detection_COLLABORA.onnx", modelLocation);
|
||||
|
||||
model_hd->input_shape.push_back(1);
|
||||
model_hd->input_shape.push_back(128);
|
||||
model_hd->input_shape.push_back(128);
|
||||
model_hd->input_shape.push_back(3);
|
||||
|
||||
model_hd->input_names.push_back("input:0");
|
||||
}
|
||||
|
||||
ORT_CHECK(g_ort, g_ort->CreateSession(htd->ort_env, modelLocation, opts, &model_hd->session));
|
||||
g_ort->ReleaseSessionOptions(opts);
|
||||
|
||||
|
||||
|
||||
model_hd->output_names.push_back("classificators");
|
||||
model_hd->output_names.push_back("regressors");
|
||||
|
||||
ORT_CHECK(g_ort, g_ort->CreateCpuMemoryInfo(OrtArenaAllocator, OrtMemTypeDefault, &model_hd->memoryInfo));
|
||||
|
||||
htv->run_detection_model = runHandDetector;
|
||||
}
|
||||
|
||||
|
||||
static void
|
||||
initOnnx(struct ht_device *htd)
|
||||
{
|
||||
htd->ort_api = OrtGetApiBase()->GetApi(ORT_API_VERSION);
|
||||
ORT_CHECK(htd->ort_api, htd->ort_api->CreateEnv(ORT_LOGGING_LEVEL_WARNING, "moses", &htd->ort_env));
|
||||
|
||||
initHandDetector(htd, &htd->views[0]);
|
||||
initHandDetector(htd, &htd->views[1]);
|
||||
|
||||
initKeypointEstimator(htd, &htd->views[0]);
|
||||
initKeypointEstimator(htd, &htd->views[1]);
|
||||
}
|
||||
|
||||
static void
|
||||
destroyModelInfo(struct ht_device *htd, ModelInfo *info)
|
||||
{
|
||||
const OrtApi *g_ort = htd->ort_api;
|
||||
g_ort->ReleaseSession(info->session);
|
||||
g_ort->ReleaseMemoryInfo(info->memoryInfo);
|
||||
// Same deal as in ht_device - I'm mixing C and C++ idioms, so sometimes it's easier to just manually call their
|
||||
// destructors instead of figuring out some way to convince C++ to call them implicitly.
|
||||
info->output_names.~vector();
|
||||
info->input_names.~vector();
|
||||
info->input_shape.~vector();
|
||||
}
|
||||
|
||||
void
|
||||
destroyOnnx(struct ht_device *htd)
|
||||
{
|
||||
destroyModelInfo(htd, &htd->views[0].keypoint_model);
|
||||
destroyModelInfo(htd, &htd->views[1].keypoint_model);
|
||||
destroyModelInfo(htd, &htd->views[0].detection_model);
|
||||
destroyModelInfo(htd, &htd->views[1].detection_model);
|
||||
htd->ort_api->ReleaseEnv(htd->ort_env);
|
||||
}
|
||||
140
src/xrt/drivers/ht/ht_nms.hpp
Normal file
140
src/xrt/drivers/ht/ht_nms.hpp
Normal file
|
|
@ -0,0 +1,140 @@
|
|||
// Copyright 2021, Collabora, Ltd.
|
||||
// SPDX-License-Identifier: BSL-1.0
|
||||
/*!
|
||||
* @file
|
||||
* @brief Code to deal with bounding boxes for camera-based hand-tracking.
|
||||
* @author Moses Turner <moses@collabora.com>
|
||||
* @author Marcus Edel <marcus.edel@collabora.com>
|
||||
* @ingroup drv_ht
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
#include <math.h>
|
||||
#include <stdio.h>
|
||||
#include <vector>
|
||||
#include <xrt/xrt_defines.h>
|
||||
#include "ht_driver.hpp"
|
||||
|
||||
struct Box
|
||||
{
|
||||
float cx;
|
||||
float cy;
|
||||
float w;
|
||||
float h;
|
||||
};
|
||||
|
||||
struct NMSPalm
|
||||
{
|
||||
Box bbox;
|
||||
xrt_vec2 keypoints[7];
|
||||
float confidence;
|
||||
};
|
||||
|
||||
static float
|
||||
overlap(float x1, float w1, float x2, float w2)
|
||||
{
|
||||
float l1 = x1 - w1 / 2;
|
||||
float l2 = x2 - w2 / 2;
|
||||
float left = l1 > l2 ? l1 : l2;
|
||||
|
||||
float r1 = x1 + w1 / 2;
|
||||
float r2 = x2 + w2 / 2;
|
||||
float right = r1 < r2 ? r1 : r2;
|
||||
|
||||
return right - left;
|
||||
}
|
||||
|
||||
static float
|
||||
boxIntersection(const Box &a, const Box &b)
|
||||
{
|
||||
float w = overlap(a.cx, a.w, b.cx, b.w);
|
||||
float h = overlap(a.cy, a.h, b.cy, b.h);
|
||||
|
||||
if (w < 0 || h < 0)
|
||||
return 0;
|
||||
|
||||
return w * h;
|
||||
}
|
||||
|
||||
static float
|
||||
boxUnion(const Box &a, const Box &b)
|
||||
{
|
||||
return a.w * a.h + b.w * b.h - boxIntersection(a, b);
|
||||
}
|
||||
|
||||
static float
|
||||
boxIOU(const Box &a, const Box &b)
|
||||
{
|
||||
return boxIntersection(a, b) / boxUnion(a, b);
|
||||
}
|
||||
|
||||
static NMSPalm
|
||||
weightedAvgBoxes(std::vector<NMSPalm> &detections)
|
||||
{
|
||||
float weight = 0.0f;
|
||||
float cx = 0.0f;
|
||||
float cy = 0.0f;
|
||||
float size = 0.0f;
|
||||
NMSPalm out = {};
|
||||
|
||||
for (NMSPalm &detection : detections) {
|
||||
weight += detection.confidence;
|
||||
cx += detection.bbox.cx * detection.confidence;
|
||||
cy += detection.bbox.cy * detection.confidence;
|
||||
size += detection.bbox.w * .5 * detection.confidence;
|
||||
size += detection.bbox.h * .5 * detection.confidence;
|
||||
|
||||
for (int i = 0; i < 7; i++) {
|
||||
out.keypoints[i].x += detection.keypoints[i].x * detection.confidence;
|
||||
out.keypoints[i].y += detection.keypoints[i].y * detection.confidence;
|
||||
}
|
||||
}
|
||||
cx /= weight;
|
||||
cy /= weight;
|
||||
size /= weight;
|
||||
for (int i = 0; i < 7; i++) {
|
||||
out.keypoints[i].x /= weight;
|
||||
out.keypoints[i].y /= weight;
|
||||
}
|
||||
out.bbox.cx = cx;
|
||||
out.bbox.cy = cy;
|
||||
out.bbox.w = size;
|
||||
out.bbox.h = size;
|
||||
return out;
|
||||
}
|
||||
|
||||
static std::vector<NMSPalm>
|
||||
filterBoxesWeightedAvg(std::vector<NMSPalm> &detections)
|
||||
{
|
||||
std::vector<std::vector<NMSPalm>> overlaps;
|
||||
std::vector<NMSPalm> outs;
|
||||
|
||||
|
||||
// U_LOG_D("\n\nStarting filtering boxes. There are %zu boxes to look at.\n", detections.size());
|
||||
for (NMSPalm &detection : detections) {
|
||||
// U_LOG_D("Starting looking at one detection\n");
|
||||
bool foundAHome = false;
|
||||
for (size_t i = 0; i < outs.size(); i++) {
|
||||
float iou = boxIOU(outs[i].bbox, detection.bbox);
|
||||
// U_LOG_D("IOU is %f\n", iou);
|
||||
// U_LOG_D("Outs box is %f %f %f %f", outs[i].bbox.cx, outs[i].bbox.cy, outs[i].bbox.w,
|
||||
// outs[i].bbox.h)
|
||||
if (iou > 0.1f) {
|
||||
// This one intersects with the whole thing
|
||||
overlaps[i].push_back(detection);
|
||||
outs[i] = weightedAvgBoxes(overlaps[i]);
|
||||
foundAHome = true;
|
||||
break;
|
||||
}
|
||||
}
|
||||
if (!foundAHome) {
|
||||
// U_LOG_D("No home\n");
|
||||
overlaps.push_back({detection});
|
||||
outs.push_back({detection});
|
||||
} else {
|
||||
// U_LOG_D("Found a home!\n");
|
||||
}
|
||||
}
|
||||
// U_LOG_D("Sizeeeeeeeeeeeeeeeeeeeee is %zu\n", outs.size());
|
||||
return outs;
|
||||
}
|
||||
|
|
@ -1,74 +0,0 @@
|
|||
// Copyright 2020, Collabora, Ltd.
|
||||
// SPDX-License-Identifier: BSL-1.0
|
||||
/*!
|
||||
* @file
|
||||
* @brief Camera based hand tracking prober code.
|
||||
* @author Christoph Haag <christoph.haag@collabora.com>
|
||||
* @ingroup drv_ht
|
||||
*/
|
||||
|
||||
|
||||
#include "xrt/xrt_prober.h"
|
||||
|
||||
#include "util/u_misc.h"
|
||||
|
||||
#include "ht_interface.h"
|
||||
#include "ht_driver.h"
|
||||
|
||||
/*!
|
||||
* @implements xrt_auto_prober
|
||||
*/
|
||||
struct ht_prober
|
||||
{
|
||||
struct xrt_auto_prober base;
|
||||
};
|
||||
|
||||
//! @private @memberof ht_prober
|
||||
static inline struct ht_prober *
|
||||
ht_prober(struct xrt_auto_prober *p)
|
||||
{
|
||||
return (struct ht_prober *)p;
|
||||
}
|
||||
|
||||
//! @public @memberof ht_prober
|
||||
static void
|
||||
ht_prober_destroy(struct xrt_auto_prober *p)
|
||||
{
|
||||
struct ht_prober *htp = ht_prober(p);
|
||||
|
||||
free(htp);
|
||||
}
|
||||
|
||||
//! @public @memberof ht_prober
|
||||
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)
|
||||
{
|
||||
struct xrt_device *xdev = ht_device_create(xap, attached_data, xp);
|
||||
|
||||
if (xdev == NULL) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
xdev->orientation_tracking_supported = true;
|
||||
xdev->position_tracking_supported = true;
|
||||
xdev->hand_tracking_supported = true;
|
||||
xdev->device_type = XRT_DEVICE_TYPE_HAND_TRACKER;
|
||||
|
||||
out_xdevs[0] = xdev;
|
||||
return 1;
|
||||
}
|
||||
|
||||
struct xrt_auto_prober *
|
||||
ht_create_auto_prober()
|
||||
{
|
||||
struct ht_prober *htp = U_TYPED_CALLOC(struct ht_prober);
|
||||
htp->base.name = "Camera Hand Tracking";
|
||||
htp->base.destroy = ht_prober_destroy;
|
||||
htp->base.lelo_dallas_autoprobe = ht_prober_autoprobe;
|
||||
|
||||
return &htp->base;
|
||||
}
|
||||
90
src/xrt/drivers/ht/readme.md
Normal file
90
src/xrt/drivers/ht/readme.md
Normal file
|
|
@ -0,0 +1,90 @@
|
|||
<!--
|
||||
Copyright 2021, Collabora, Ltd.
|
||||
Authors:
|
||||
Moses Turner <moses@collabora.com>
|
||||
SPDX-License-Identifier: BSL-1.0
|
||||
-->
|
||||
|
||||
# What is this?
|
||||
This is a driver to do optical hand tracking. The actual code mostly written by Moses Turner, with tons of help from Marcus Edel, Jakob Bornecrantz, Ryan Pavlik, and Christoph Haag. Jakob Bornecrantz and Marcus Edel are the main people who gathered training data for the initial Collabora models.
|
||||
|
||||
Currently, it works with the Valve Index. In the past, it was tested with a Luxonis 1090ffc, and in the future it should work fine with devices like the T265, Leap Motion Controller (w/ LeapUVC), or PS4/PS5 cam, should there be enough interest for any of those.
|
||||
|
||||
Under good lighting, I would say it's around as good as Oculus Quest 2's hand tracking. Not that I'm trying to make any claims; that's just what I honestly would tell somebody if they are wondering if it's worth testing out.
|
||||
|
||||
|
||||
# How to get started
|
||||
## Get dependencies
|
||||
### Get OpenCV
|
||||
Each distro has its own way to get OpenCV, and it can change at any time; there's no specific reason to trust this documentation over anything else.
|
||||
|
||||
Having said that, on Ubuntu, it would look something like
|
||||
|
||||
```
|
||||
sudo apt install libopencv-dev libopencv-contrib-dev
|
||||
```
|
||||
|
||||
Or you could build it from source, or get it from one of the other 1000s of package managers. Whatever floats your boat.
|
||||
|
||||
### Get ONNXRuntime
|
||||
I followed the instructions here: https://onnxruntime.ai/docs/how-to/build/inferencing.html#linux
|
||||
|
||||
then had to do
|
||||
```
|
||||
cd build/Linux/RelWithDebInfo/
|
||||
sudo make install
|
||||
```
|
||||
|
||||
### Get the ML models
|
||||
Make sure you have git-lfs installed, then run ./scripts/get-ht-models.sh. Should work fine.
|
||||
|
||||
## Building the driver
|
||||
Once onnxruntime is installed, you should be able to build like normal with CMake or Meson.
|
||||
|
||||
If it properly found everything, - CMake should say
|
||||
|
||||
```
|
||||
-- Found ONNXRUNTIME: /usr/local/include/onnxruntime
|
||||
|
||||
[...]
|
||||
|
||||
-- # DRIVER_HANDTRACKING: ON
|
||||
```
|
||||
|
||||
and Meson should say
|
||||
|
||||
```
|
||||
Run-time dependency libonnxruntime found: YES 1.8.2
|
||||
|
||||
[...]
|
||||
|
||||
Message: Configuration done!
|
||||
Message: drivers: [...] handtracking, [...]
|
||||
```
|
||||
|
||||
## Running the driver
|
||||
Currently, it's only set up to work on Valve Index.
|
||||
|
||||
So, the two things you can do are
|
||||
* Use the `survive` driver with both controllers off - It should automagically start hand tracking upon not finding any controllers.
|
||||
* Use the `vive` driver with `VIVE_USE_HANDTRACKING=ON` and it should work the same as the survive driver.
|
||||
|
||||
You can see if the driver is working with `openxr-simple-playground`, StereoKit, or any other app you know of. Poke me (Moses) if you find any other cool hand-tracking apps; I'm always looking for more!
|
||||
|
||||
# Tips and tricks
|
||||
|
||||
This tracking likes to be in a bright, evenly-lit room with multiple light sources. Turn all the lights on, see if you can find any lamps. If the ML models can see well, the tracking quality can get surprisingly nice.
|
||||
|
||||
Sometimes, the tracking fails when it can see more than one hand. As the tracking gets better (we train better ML models and squash more bugs) this should happen less often or not at all. If it does, put one of your hands down, and it should resume tracking the remaining hand just fine.
|
||||
|
||||
# Future improvements
|
||||
|
||||
* Get more training data; train better ML models.
|
||||
* Improve the tracking math
|
||||
* Be smarter about keeping tracking lock on a hand
|
||||
* Try predicting the next bounding box based on the estimated keypoints of the last few frames instead of blindly trusting the detection model, and not run the detection model *every single* frame.
|
||||
* Instead of directly doing disparity on the observed keypoints, use a kinematic model of the hand and fit that to the 2D observations - this should get rid of a *lot* of jitter and make it look better to the end user if the ML models fail
|
||||
* Make something that also works with non-stereo (mono, trinocular, or N cameras) camera setups
|
||||
* Optionally run the ML models on GPU - currently, everything's CPU bound which could be dumb under some circumstances
|
||||
* Write a lot of generic code so that you can run this on any stereo camera
|
||||
* More advanced prediction/interpolation code that doesn't care at all about the input frame cadence. One-euro filters are pretty good about this, but we can get better!
|
||||
64
src/xrt/drivers/ht/templates/DiscardLastBuffer.hpp
Normal file
64
src/xrt/drivers/ht/templates/DiscardLastBuffer.hpp
Normal file
|
|
@ -0,0 +1,64 @@
|
|||
// Copyright 2021, Collabora, Ltd.
|
||||
// SPDX-License-Identifier: BSL-1.0
|
||||
/*!
|
||||
* @file
|
||||
* @brief Camera based hand tracking ringbuffer implementation.
|
||||
* @author Moses Turner <moses@collabora.com>
|
||||
* @ingroup drv_ht
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <stdio.h>
|
||||
#include <stdlib.h>
|
||||
#include <stdint.h>
|
||||
#include <assert.h>
|
||||
|
||||
|
||||
//| -4 | -3 | -2 | -1 | Top | Garbage |
|
||||
// OR
|
||||
//| -4 | -3 | -2 | -1 | Top | -7 | -6 | -5 |
|
||||
|
||||
|
||||
|
||||
template <typename T, int maxSize> struct DiscardLastBuffer
|
||||
{
|
||||
T internalBuffer[maxSize];
|
||||
int topIdx = 0;
|
||||
|
||||
/* Put something at the top, overwrite whatever was at the back*/
|
||||
void
|
||||
push(const T inElement);
|
||||
|
||||
T *operator[](int inIndex);
|
||||
};
|
||||
|
||||
template <typename T, int maxSize>
|
||||
void
|
||||
DiscardLastBuffer<T, maxSize>::push(const T inElement)
|
||||
{
|
||||
topIdx++;
|
||||
if (topIdx == maxSize) {
|
||||
topIdx = 0;
|
||||
}
|
||||
|
||||
memcpy(&internalBuffer[topIdx], &inElement, sizeof(T));
|
||||
}
|
||||
|
||||
template <typename T, int maxSize> T *DiscardLastBuffer<T, maxSize>::operator[](int inIndex)
|
||||
{
|
||||
assert(inIndex <= maxSize);
|
||||
assert(inIndex >= 0);
|
||||
|
||||
int index = topIdx - inIndex;
|
||||
if (index < 0) {
|
||||
index = maxSize + index;
|
||||
}
|
||||
|
||||
assert(index >= 0);
|
||||
if (index > maxSize) {
|
||||
assert(false);
|
||||
}
|
||||
|
||||
return &internalBuffer[index];
|
||||
}
|
||||
90
src/xrt/drivers/ht/templates/NaivePermutationSort.hpp
Normal file
90
src/xrt/drivers/ht/templates/NaivePermutationSort.hpp
Normal file
|
|
@ -0,0 +1,90 @@
|
|||
// Copyright 2021, Collabora, Ltd.
|
||||
// SPDX-License-Identifier: BSL-1.0
|
||||
/*!
|
||||
* @file
|
||||
* @brief Camera based hand tracking sorting implementation.
|
||||
* @author Moses Turner <moses@collabora.com>
|
||||
* @ingroup drv_ht
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <math.h>
|
||||
#include <vector>
|
||||
#include <algorithm>
|
||||
#include <iostream>
|
||||
// Other thing: sort by speed? like, if our thing must have suddenly changed directions, add to error?
|
||||
// Easy enough to do using more complicated structs.
|
||||
// Like a past thing with position, velocity and timestamp - present thing with position and timestamp.
|
||||
|
||||
// typedef bool booool;
|
||||
|
||||
struct psort_atom_t
|
||||
{
|
||||
size_t idx_1;
|
||||
size_t idx_2;
|
||||
float err;
|
||||
};
|
||||
|
||||
|
||||
bool
|
||||
comp_err(psort_atom_t one, psort_atom_t two)
|
||||
{
|
||||
return (one.err < two.err);
|
||||
}
|
||||
|
||||
|
||||
template <typename Tp_1, typename Tp_2>
|
||||
void
|
||||
naive_sort_permutation_by_error(
|
||||
// Inputs - shall be initialized with real data before calling. This function shall not modify them in any way.
|
||||
std::vector<Tp_1> &in_1,
|
||||
std::vector<Tp_2> &in_2,
|
||||
|
||||
// Outputs - shall be uninitialized. This function shall initialize them to the right size and fill them with the
|
||||
// proper values.
|
||||
std::vector<bool> &used_1,
|
||||
std::vector<bool> &used_2,
|
||||
std::vector<size_t> &out_indices_1,
|
||||
std::vector<size_t> &out_indices_2,
|
||||
std::vector<float> &out_errs,
|
||||
|
||||
float (*calc_error)(Tp_1 *one, Tp_2 *two))
|
||||
{
|
||||
used_1 = std::vector<bool>(in_1.size()); // silly? Unsure.
|
||||
used_2 = std::vector<bool>(in_2.size());
|
||||
|
||||
size_t out_size = std::min(in_1.size(), in_2.size());
|
||||
|
||||
out_indices_1.reserve(out_size);
|
||||
out_indices_2.reserve(out_size);
|
||||
|
||||
std::vector<psort_atom_t> associations;
|
||||
|
||||
for (size_t idx_1 = 0; idx_1 < in_1.size(); idx_1++) {
|
||||
for (size_t idx_2 = 0; idx_2 < in_2.size(); idx_2++) {
|
||||
float err = calc_error(&in_1[idx_1], &in_2[idx_2]);
|
||||
if (err > 0.0f) {
|
||||
// Negative error means the error calculator thought there was something so bad with
|
||||
// these that they shouldn't be considered at all.
|
||||
associations.push_back({idx_1, idx_2, err});
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
std::sort(associations.begin(), associations.end(), comp_err);
|
||||
|
||||
for (size_t i = 0; i < associations.size(); i++) {
|
||||
psort_atom_t chonk = associations[i];
|
||||
if (used_1[chonk.idx_1] || used_2[chonk.idx_2]) {
|
||||
continue;
|
||||
}
|
||||
used_1[chonk.idx_1] = true;
|
||||
used_2[chonk.idx_2] = true;
|
||||
|
||||
out_indices_1.push_back(chonk.idx_1);
|
||||
out_indices_2.push_back(chonk.idx_2);
|
||||
|
||||
out_errs.push_back(chonk.err);
|
||||
}
|
||||
}
|
||||
|
|
@ -87,13 +87,18 @@ lib_drv_ulv2 = static_library(
|
|||
lib_drv_ht = static_library(
|
||||
'drv_ht',
|
||||
files(
|
||||
'ht/ht_driver.c',
|
||||
'ht/ht_driver.h',
|
||||
'ht/ht_driver.cpp',
|
||||
'ht/ht_driver.hpp',
|
||||
'ht/ht_interface.h',
|
||||
'ht/ht_prober.c',
|
||||
'ht/ht_models.hpp',
|
||||
'ht/ht_hand_math.hpp',
|
||||
'ht/ht_image_math.hpp',
|
||||
'ht/ht_nms.hpp',
|
||||
'ht/templates/DiscardLastBuffer.hpp',
|
||||
'ht/templates/NaivePermutationSort.hpp',
|
||||
),
|
||||
include_directories: xrt_include,
|
||||
dependencies: [aux],
|
||||
include_directories: [xrt_include, cjson_include],
|
||||
dependencies: [aux, opencv, onnxruntime, eigen3],
|
||||
build_by_default: 'handtracking' in drivers,
|
||||
)
|
||||
|
||||
|
|
|
|||
Loading…
Reference in a new issue