// 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 * @author Nick Klingensmith * @ingroup drv_ht */ #pragma once #include "math/m_api.h" #include "math/m_vec3.h" #include "ht_driver.hpp" 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> 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 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 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; }