-2

src/xrt/tracking/hand/mercury/CMakeLists.txt

reviewed

··· 4 4 # Mercury hand tracking library! 5 5 6 6 add_subdirectory(kine_lm) 7 7 - add_subdirectory(kine_ccdik) 8 7 9 8 add_library(t_ht_mercury_model STATIC hg_model.cpp) 10 9 ··· 37 36 aux_util 38 37 ONNXRuntime::ONNXRuntime 39 38 t_ht_mercury_kine_lm 40 40 - t_ht_mercury_kine_ccdik 41 39 t_ht_mercury_model 42 40 # ncnn # Soon... 43 41 ${OpenCV_LIBRARIES}

+26 -45

src/xrt/tracking/hand/mercury/hg_sync.cpp

reviewed

··· 664 664 lm::optimizer_destroy(&this->kinematic_hands[0]); 665 665 lm::optimizer_destroy(&this->kinematic_hands[1]); 666 666 667 667 - ccdik::free_kinematic_hand(&this->kinematic_hands_ccdik[0]); 668 668 - ccdik::free_kinematic_hand(&this->kinematic_hands_ccdik[1]); 669 669 - 670 667 u_var_remove_root((void *)&this->base); 671 668 u_frame_times_widget_teardown(&this->ft_widget); 672 669 } ··· 886 883 887 884 struct xrt_hand_joint_set *put_in_set = out_xrt_hands[hand_idx]; 888 885 889 889 - if (__builtin_expect(!hgt->tuneable_values.use_ccdik, true)) { 890 890 - lm::KinematicHandLM *hand = hgt->kinematic_hands[hand_idx]; 886 886 + lm::KinematicHandLM *hand = hgt->kinematic_hands[hand_idx]; 891 887 892 892 - //!@todo 893 893 - // ABOUT TWO MINUTES OF THOUGHT WERE PUT INTO THIS VALUE 894 894 - float reprojection_error_threshold = 0.35f; 888 888 + //!@todo 889 889 + // ABOUT TWO MINUTES OF THOUGHT WERE PUT INTO THIS VALUE 890 890 + float reprojection_error_threshold = 0.35f; 895 891 896 896 - float out_hand_size; 892 892 + float out_hand_size; 897 893 898 898 - //!@todo Optimize: We can have one of these on each thread 899 899 - float reprojection_error; 900 900 - lm::optimizer_run(hand, // 901 901 - input, // 902 902 - !hgt->last_frame_hand_detected[hand_idx], // 903 903 - optimize_hand_size, // 904 904 - hgt->target_hand_size, // 905 905 - hgt->refinement.hand_size_refinement_schedule_y, // 906 906 - *put_in_set, // 907 907 - out_hand_size, // 908 908 - reprojection_error); 894 894 + //!@todo Optimize: We can have one of these on each thread 895 895 + float reprojection_error; 896 896 + lm::optimizer_run(hand, // 897 897 + input, // 898 898 + !hgt->last_frame_hand_detected[hand_idx], // 899 899 + optimize_hand_size, // 900 900 + hgt->target_hand_size, // 901 901 + hgt->refinement.hand_size_refinement_schedule_y, // 902 902 + *put_in_set, // 903 903 + out_hand_size, // 904 904 + reprojection_error); 909 905 910 910 - avg_hand_size += out_hand_size; 911 911 - num_hands++; 906 906 + avg_hand_size += out_hand_size; 907 907 + num_hands++; 912 908 913 913 - if (reprojection_error > reprojection_error_threshold) { 914 914 - HG_DEBUG(hgt, "Reprojection error above threshold!"); 915 915 - hgt->this_frame_hand_detected[hand_idx] = false; 909 909 + if (reprojection_error > reprojection_error_threshold) { 910 910 + HG_DEBUG(hgt, "Reprojection error above threshold!"); 911 911 + hgt->this_frame_hand_detected[hand_idx] = false; 916 912 917 917 - continue; 918 918 - } 919 919 - if (!any_hands_are_only_visible_in_one_view) { 920 920 - hgt->refinement.hand_size_refinement_schedule_x += 921 921 - hand_confidence_value(reprojection_error, input); 922 922 - } 923 923 - 924 924 - } else { 925 925 - ccdik::KinematicHandCCDIK *hand = hgt->kinematic_hands_ccdik[hand_idx]; 926 926 - if (!hgt->last_frame_hand_detected[hand_idx]) { 927 927 - ccdik::init_hardcoded_statics(hand, hgt->target_hand_size); 928 928 - } 929 929 - ccdik::optimize_new_frame(hand, input, *put_in_set); 913 913 + continue; 914 914 + } 915 915 + if (!any_hands_are_only_visible_in_one_view) { 916 916 + hgt->refinement.hand_size_refinement_schedule_x += 917 917 + hand_confidence_value(reprojection_error, input); 930 918 } 931 931 - 932 932 - 933 919 934 920 u_hand_joints_apply_joint_width(put_in_set); 935 921 ··· 1053 1039 lm::optimizer_create(hgt->left_in_right, false, hgt->log_level, &hgt->kinematic_hands[0]); 1054 1040 lm::optimizer_create(hgt->left_in_right, true, hgt->log_level, &hgt->kinematic_hands[1]); 1055 1041 1056 1056 - ccdik::alloc_kinematic_hand(hgt->left_in_right, false, &hgt->kinematic_hands_ccdik[0]); 1057 1057 - ccdik::alloc_kinematic_hand(hgt->left_in_right, true, &hgt->kinematic_hands_ccdik[1]); 1058 1058 - 1059 1042 u_frame_times_widget_init(&hgt->ft_widget, 10.0f, 10.0f); 1060 1043 1061 1044 u_var_add_root(hgt, "Camera-based Hand Tracker", true); ··· 1100 1083 u_var_add_bool(hgt, &hgt->tuneable_values.scribble_keypoint_model_outputs, "Scribble keypoint model output"); 1101 1084 u_var_add_bool(hgt, &hgt->tuneable_values.scribble_optimizer_outputs, "Scribble kinematic optimizer output"); 1102 1085 u_var_add_bool(hgt, &hgt->tuneable_values.always_run_detection_model, "Always run detection model"); 1103 1103 - u_var_add_bool(hgt, &hgt->tuneable_values.use_ccdik, 1104 1104 - "Use IK optimizer (may put tracking in unexpected state, use with care)"); 1105 1086 1106 1087 1107 1088 u_var_add_sink_debug(hgt, &hgt->debug_sink_ann, "Annotated camera feeds");

-3

src/xrt/tracking/hand/mercury/hg_sync.hpp

reviewed

··· 46 46 47 47 #include "kine_common.hpp" 48 48 #include "kine_lm/lm_interface.hpp" 49 49 - #include "kine_ccdik/ccdik_interface.hpp" 50 49 51 50 52 51 ··· 240 239 enum u_logging_level log_level = U_LOGGING_INFO; 241 240 242 241 lm::KinematicHandLM *kinematic_hands[2]; 243 243 - ccdik::KinematicHandCCDIK *kinematic_hands_ccdik[2]; 244 242 245 243 // struct hand_detection_state_tracker st_det[2] = {}; 246 244 bool hand_seen_before[2] = {false, false}; ··· 277 275 bool scribble_keypoint_model_outputs = false; 278 276 bool scribble_optimizer_outputs = true; 279 277 bool always_run_detection_model = false; 280 280 - bool use_ccdik = false; 281 278 int max_num_outside_view = 3; 282 279 } tuneable_values; 283 280

-8

src/xrt/tracking/hand/mercury/kine_ccdik/CMakeLists.txt

reviewed

··· 1 1 - # Copyright 2022, Collabora, Ltd. 2 2 - # SPDX-License-Identifier: BSL-1.0 3 3 - 4 4 - add_library(t_ht_mercury_kine_ccdik STATIC ccdik_interface.hpp ccdik_main.cpp) 5 5 - 6 6 - target_link_libraries(t_ht_mercury_kine_ccdik PRIVATE aux_math aux_tracking aux_os aux_util) 7 7 - 8 8 - target_include_directories(t_ht_mercury_kine_ccdik SYSTEM PRIVATE ${EIGEN3_INCLUDE_DIR})

-178

src/xrt/tracking/hand/mercury/kine_ccdik/ccdik_defines.hpp

reviewed

··· 1 1 - // Copyright 2022, Collabora, Ltd. 2 2 - // SPDX-License-Identifier: BSL-1.0 3 3 - /*! 4 4 - * @file 5 5 - * @brief Defines for kinematic model 6 6 - * @author Moses Turner <moses@collabora.com> 7 7 - * @ingroup tracking 8 8 - */ 9 9 - 10 10 - 11 11 - #pragma once 12 12 - #include "math/m_api.h" 13 13 - 14 14 - 15 15 - #include <stddef.h> 16 16 - #include <unistd.h> 17 17 - 18 18 - #include <Eigen/Core> 19 19 - #include <Eigen/Geometry> 20 20 - 21 21 - #include "math/m_eigen_interop.hpp" 22 22 - #include "math/m_api.h" 23 23 - #include "math/m_space.h" 24 24 - #include "math/m_vec3.h" 25 25 - 26 26 - #include "util/u_trace_marker.h" 27 27 - 28 28 - #include "os/os_time.h" 29 29 - #include "util/u_time.h" 30 30 - 31 31 - #include <iostream> 32 32 - #include <vector> 33 33 - 34 34 - #include <stdio.h> 35 35 - 36 36 - #include <stddef.h> 37 37 - #include <unistd.h> 38 38 - #include "../kine_common.hpp" 39 39 - 40 40 - 41 41 - using namespace xrt::auxiliary::math; 42 42 - 43 43 - namespace xrt::tracking::hand::mercury::ccdik { 44 44 - 45 45 - enum class HandJoint26KP : int 46 46 - { 47 47 - PALM = 0, 48 48 - WRIST = 1, 49 49 - 50 50 - THUMB_METACARPAL = 2, 51 51 - THUMB_PROXIMAL = 3, 52 52 - THUMB_DISTAL = 4, 53 53 - THUMB_TIP = 5, 54 54 - 55 55 - INDEX_METACARPAL = 6, 56 56 - INDEX_PROXIMAL = 7, 57 57 - INDEX_INTERMEDIATE = 8, 58 58 - INDEX_DISTAL = 9, 59 59 - INDEX_TIP = 10, 60 60 - 61 61 - MIDDLE_METACARPAL = 11, 62 62 - MIDDLE_PROXIMAL = 12, 63 63 - MIDDLE_INTERMEDIATE = 13, 64 64 - MIDDLE_DISTAL = 14, 65 65 - MIDDLE_TIP = 15, 66 66 - 67 67 - RING_METACARPAL = 16, 68 68 - RING_PROXIMAL = 17, 69 69 - RING_INTERMEDIATE = 18, 70 70 - RING_DISTAL = 19, 71 71 - RING_TIP = 20, 72 72 - 73 73 - LITTLE_METACARPAL = 21, 74 74 - LITTLE_PROXIMAL = 22, 75 75 - LITTLE_INTERMEDIATE = 23, 76 76 - LITTLE_DISTAL = 24, 77 77 - LITTLE_TIP = 25, 78 78 - }; 79 79 - 80 80 - enum HandFinger 81 81 - { 82 82 - HF_THUMB = 0, 83 83 - HF_INDEX = 1, 84 84 - HF_MIDDLE = 2, 85 85 - HF_RING = 3, 86 86 - HF_LITTLE = 4, 87 87 - }; 88 88 - 89 89 - enum FingerBone 90 90 - { 91 91 - // FB_CARPAL, 92 92 - FB_METACARPAL, 93 93 - FB_PROXIMAL, 94 94 - FB_INTERMEDIATE, 95 95 - FB_DISTAL 96 96 - }; 97 97 - 98 98 - enum ThumbBone 99 99 - { 100 100 - // TB_CARPAL, 101 101 - TB_METACARPAL, 102 102 - TB_PROXIMAL, 103 103 - TB_DISTAL 104 104 - }; 105 105 - 106 106 - const size_t kNumNNJoints = 21; 107 107 - 108 108 - struct wct_t 109 109 - { 110 110 - float waggle = 0.0f; 111 111 - float curl = 0.0f; 112 112 - float twist = 0.0f; 113 113 - }; 114 114 - 115 115 - // IGNORE THE FIRST BONE in the wrist. 116 116 - struct bone_t 117 117 - { 118 118 - // EIGEN_OVERRIDE_OPERATOR_NEW 119 119 - EIGEN_MAKE_ALIGNED_OPERATOR_NEW 120 120 - // will always be 0, 0, -(some amount) for mcp, pxm, int, dst 121 121 - // will be random amounts for carpal bones 122 122 - Eigen::Vector3f trans_from_last_joint = Eigen::Vector3f::Zero(); 123 123 - wct_t rot_to_next_joint_wct = {}; 124 124 - Eigen::Quaternionf rot_to_next_joint_quat = {}; 125 125 - // Translation from last joint to this joint, rotation that takes last joint's -z and points it at next joint 126 126 - Eigen::Affine3f bone_relation = {}; 127 127 - 128 128 - // Imagine it like transforming an object at the origin to this bone's position/orientation. 129 129 - Eigen::Affine3f world_pose = {}; 130 130 - 131 131 - struct 132 132 - { 133 133 - Eigen::Affine3f *world_pose; 134 134 - Eigen::Affine3f *bone_relation; 135 135 - } parent; 136 136 - 137 137 - 138 138 - wct_t joint_limit_min = {}; 139 139 - wct_t joint_limit_max = {}; 140 140 - 141 141 - 142 142 - // What keypoint out of the ML model does this correspond to? 143 143 - Joint21::Joint21 keypoint_idx_21 = {}; 144 144 - }; 145 145 - 146 146 - // translation: wrist to mcp (-z and x). rotation: from wrist space to metacarpal space 147 147 - // translation: mcp to pxm (just -z). rotation: from mcp to pxm space 148 148 - 149 149 - struct finger_t 150 150 - { 151 151 - bone_t bones[5] = {}; 152 152 - }; 153 153 - 154 154 - 155 155 - struct KinematicHandCCDIK 156 156 - { 157 157 - // The distance from the wrist to the middle-proximal joint - this sets the overall hand size. 158 158 - float size; 159 159 - bool is_right; 160 160 - xrt_pose right_in_left; 161 161 - 162 162 - // Wrist pose, scaled by size. 163 163 - Eigen::Affine3f wrist_relation = {}; 164 164 - 165 165 - finger_t fingers[5] = {}; 166 166 - 167 167 - xrt_vec3 t_jts[kNumNNJoints] = {}; 168 168 - Eigen::Matrix<float, 3, kNumNNJoints> t_jts_as_mat = {}; 169 169 - Eigen::Matrix<float, 3, kNumNNJoints> kinematic = {}; 170 170 - }; 171 171 - 172 172 - 173 173 - #define CONTINUE_IF_HIDDEN_THUMB \ 174 174 - if (finger_idx == 0 && bone_idx == 0) { \ 175 175 - continue; \ 176 176 - } 177 177 - 178 178 - } // namespace xrt::tracking::hand::mercury::ccdik

-232

src/xrt/tracking/hand/mercury/kine_ccdik/ccdik_hand_init.hpp

reviewed

··· 1 1 - // Copyright 2022, Collabora, Ltd. 2 2 - // SPDX-License-Identifier: BSL-1.0 3 3 - /*! 4 4 - * @file 5 5 - * @brief Setter-upper for kinematic model 6 6 - * @author Moses Turner <moses@collabora.com> 7 7 - * @ingroup tracking 8 8 - */ 9 9 - 10 10 - 11 11 - #pragma once 12 12 - 13 13 - #include "ccdik_defines.hpp" 14 14 - #include "ccdik_tiny_math.hpp" 15 15 - 16 16 - 17 17 - namespace xrt::tracking::hand::mercury::ccdik { 18 18 - 19 19 - void 20 20 - bone_update_quat_and_matrix(struct bone_t *bone) 21 21 - { 22 22 - wct_to_quat(bone->rot_to_next_joint_wct, (xrt_quat *)&bone->rot_to_next_joint_quat); 23 23 - 24 24 - // To make sure that the bottom-right cell is 1.0f. You can do this somewhere else (just once) for speed 25 25 - bone->bone_relation.setIdentity(); 26 26 - bone->bone_relation.translation() = bone->trans_from_last_joint; 27 27 - bone->bone_relation.linear() = Eigen::Matrix3f(bone->rot_to_next_joint_quat); 28 28 - } 29 29 - 30 30 - void 31 31 - eval_chain(std::vector<const Eigen::Affine3f *> &chain, Eigen::Affine3f &out) 32 32 - { 33 33 - out.setIdentity(); 34 34 - for (const Eigen::Affine3f *new_matrix : chain) { 35 35 - // I have NO IDEA if it's correct to left-multiply or right-multiply. 36 36 - // I need to go to school for linear algebra for a lot longer... 37 37 - out = out * (*new_matrix); 38 38 - } 39 39 - } 40 40 - 41 41 - void 42 42 - _statics_init_world_parents(KinematicHandCCDIK *hand) 43 43 - { 44 44 - for (int finger = 0; finger < 5; finger++) { 45 45 - finger_t *of = &hand->fingers[finger]; 46 46 - of->bones[0].parent.world_pose = &hand->wrist_relation; 47 47 - 48 48 - // Does this make any sense? Be careful here... 49 49 - of->bones[0].parent.bone_relation = &hand->wrist_relation; 50 50 - 51 51 - for (int bone = 1; bone < 5; bone++) { 52 52 - of->bones[bone].parent.world_pose = &of->bones[bone - 1].world_pose; 53 53 - of->bones[bone].parent.bone_relation = &of->bones[bone - 1].bone_relation; 54 54 - } 55 55 - } 56 56 - } 57 57 - 58 58 - void 59 59 - _statics_init_world_poses(KinematicHandCCDIK *hand) 60 60 - { 61 61 - XRT_TRACE_MARKER(); 62 62 - for (int finger = 0; finger < 5; finger++) { 63 63 - finger_t *of = &hand->fingers[finger]; 64 64 - 65 65 - for (int bone = 0; bone < 5; bone++) { 66 66 - of->bones[bone].world_pose = 67 67 - (*of->bones[bone].parent.world_pose) * of->bones[bone].bone_relation; 68 68 - } 69 69 - } 70 70 - } 71 71 - 72 72 - void 73 73 - _statics_init_loc_ptrs(KinematicHandCCDIK *hand) 74 74 - { 75 75 - hand->fingers[0].bones[1].keypoint_idx_21 = Joint21::THMB_MCP; 76 76 - hand->fingers[0].bones[2].keypoint_idx_21 = Joint21::THMB_PXM; 77 77 - hand->fingers[0].bones[3].keypoint_idx_21 = Joint21::THMB_DST; 78 78 - hand->fingers[0].bones[4].keypoint_idx_21 = Joint21::THMB_TIP; 79 79 - 80 80 - hand->fingers[1].bones[1].keypoint_idx_21 = Joint21::INDX_PXM; 81 81 - hand->fingers[1].bones[2].keypoint_idx_21 = Joint21::INDX_INT; 82 82 - hand->fingers[1].bones[3].keypoint_idx_21 = Joint21::INDX_DST; 83 83 - hand->fingers[1].bones[4].keypoint_idx_21 = Joint21::INDX_TIP; 84 84 - 85 85 - hand->fingers[2].bones[1].keypoint_idx_21 = Joint21::MIDL_PXM; 86 86 - hand->fingers[2].bones[2].keypoint_idx_21 = Joint21::MIDL_INT; 87 87 - hand->fingers[2].bones[3].keypoint_idx_21 = Joint21::MIDL_DST; 88 88 - hand->fingers[2].bones[4].keypoint_idx_21 = Joint21::MIDL_TIP; 89 89 - 90 90 - hand->fingers[3].bones[1].keypoint_idx_21 = Joint21::RING_PXM; 91 91 - hand->fingers[3].bones[2].keypoint_idx_21 = Joint21::RING_INT; 92 92 - hand->fingers[3].bones[3].keypoint_idx_21 = Joint21::RING_DST; 93 93 - hand->fingers[3].bones[4].keypoint_idx_21 = Joint21::RING_TIP; 94 94 - 95 95 - hand->fingers[4].bones[1].keypoint_idx_21 = Joint21::LITL_PXM; 96 96 - hand->fingers[4].bones[2].keypoint_idx_21 = Joint21::LITL_INT; 97 97 - hand->fingers[4].bones[3].keypoint_idx_21 = Joint21::LITL_DST; 98 98 - hand->fingers[4].bones[4].keypoint_idx_21 = Joint21::LITL_TIP; 99 99 - } 100 100 - 101 101 - void 102 102 - _statics_joint_limits(KinematicHandCCDIK *hand) 103 103 - { 104 104 - { 105 105 - finger_t *t = &hand->fingers[0]; 106 106 - t->bones[1].joint_limit_max.waggle = rad(70); 107 107 - t->bones[1].joint_limit_min.waggle = rad(-70); 108 108 - 109 109 - t->bones[1].joint_limit_max.curl = rad(70); 110 110 - t->bones[1].joint_limit_min.curl = rad(-70); 111 111 - 112 112 - t->bones[1].joint_limit_max.twist = rad(40); 113 113 - t->bones[1].joint_limit_min.twist = rad(-40); 114 114 - 115 115 - // 116 116 - 117 117 - t->bones[2].joint_limit_max.waggle = rad(0); 118 118 - t->bones[2].joint_limit_min.waggle = rad(0); 119 119 - 120 120 - t->bones[2].joint_limit_max.curl = rad(50); 121 121 - t->bones[2].joint_limit_min.curl = rad(-100); 122 122 - 123 123 - t->bones[2].joint_limit_max.twist = rad(0); 124 124 - t->bones[2].joint_limit_min.twist = rad(0); 125 125 - 126 126 - // 127 127 - 128 128 - t->bones[3].joint_limit_max.waggle = rad(0); 129 129 - t->bones[3].joint_limit_min.waggle = rad(0); 130 130 - 131 131 - t->bones[3].joint_limit_max.curl = rad(50); 132 132 - t->bones[3].joint_limit_min.curl = rad(-100); 133 133 - 134 134 - t->bones[3].joint_limit_max.twist = rad(0); 135 135 - t->bones[3].joint_limit_min.twist = rad(0); 136 136 - } 137 137 - } 138 138 - 139 139 - // Exported: 140 140 - void 141 141 - init_hardcoded_statics(KinematicHandCCDIK *hand, float size) 142 142 - { 143 143 - hand->size = size; 144 144 - hand->wrist_relation.setIdentity(); 145 145 - hand->wrist_relation.linear() *= hand->size; 146 146 - 147 147 - { 148 148 - 149 149 - finger_t *t = &hand->fingers[0]; 150 150 - 151 151 - // Hidden extra bone that makes our code easier to write. Note the weird extra rotation. 152 152 - t->bones[0].rot_to_next_joint_wct = {-rad(45), rad(-10), -rad(70)}; 153 153 - t->bones[0].trans_from_last_joint = {0.33097, 0, -0.25968}; 154 154 - 155 155 - t->bones[1].rot_to_next_joint_wct = {0, rad(-5), 0}; 156 156 - 157 157 - t->bones[2].rot_to_next_joint_wct = {0, rad(-25), 0}; 158 158 - t->bones[2].trans_from_last_joint.z() = -0.389626; 159 159 - 160 160 - t->bones[3].rot_to_next_joint_wct = {0, rad(-25), 0}; 161 161 - t->bones[3].trans_from_last_joint.z() = -0.311176; 162 162 - 163 163 - t->bones[4].trans_from_last_joint.z() = -0.232195; 164 164 - } 165 165 - 166 166 - 167 167 - float wagg = -0.19; 168 168 - 169 169 - float finger_joints[4][3] = { 170 170 - { 171 171 - -0.365719, 172 172 - -0.231581, 173 173 - -0.201790, 174 174 - }, 175 175 - { 176 176 - -0.404486, 177 177 - -0.247749, 178 178 - -0.210121, 179 179 - }, 180 180 - { 181 181 - -0.365639, 182 182 - -0.225666, 183 183 - -0.187089, 184 184 - }, 185 185 - { 186 186 - -0.278197, 187 187 - -0.176178, 188 188 - -0.157566, 189 189 - }, 190 190 - }; 191 191 - 192 192 - for (int finger = HF_INDEX; finger <= HF_LITTLE; finger++) { 193 193 - finger_t *of = &hand->fingers[finger]; 194 194 - of->bones[0].rot_to_next_joint_wct.waggle = wagg; 195 195 - wagg += 0.19f; 196 196 - 197 197 - of->bones[FB_PROXIMAL].rot_to_next_joint_wct.curl = rad(-5); 198 198 - of->bones[FB_INTERMEDIATE].rot_to_next_joint_wct.curl = rad(-5); 199 199 - of->bones[FB_DISTAL].rot_to_next_joint_wct.curl = rad(-5); 200 200 - 201 201 - 202 202 - for (int i = 0; i < 3; i++) { 203 203 - int bone = i + 2; 204 204 - of->bones[bone].trans_from_last_joint.x() = 0; 205 205 - of->bones[bone].trans_from_last_joint.y() = 0; 206 206 - of->bones[bone].trans_from_last_joint.z() = finger_joints[finger - 1][i]; 207 207 - } 208 208 - } 209 209 - 210 210 - 211 211 - hand->fingers[1].bones[1].trans_from_last_joint.z() = -0.66; 212 212 - hand->fingers[2].bones[1].trans_from_last_joint.z() = -0.645; 213 213 - hand->fingers[3].bones[1].trans_from_last_joint.z() = -0.58; 214 214 - hand->fingers[4].bones[1].trans_from_last_joint.z() = -0.52; 215 215 - 216 216 - hand->fingers[HF_INDEX].bones[0].trans_from_last_joint = {0.16926, 0, -0.34437}; 217 217 - hand->fingers[HF_MIDDLE].bones[0].trans_from_last_joint = {0.034639, 0, -0.35573}; 218 218 - hand->fingers[HF_RING].bones[0].trans_from_last_joint = {-0.063625, 0, -0.34164}; 219 219 - hand->fingers[HF_LITTLE].bones[0].trans_from_last_joint = {-0.1509, 0, -0.30373}; 220 220 - 221 221 - for (int finger_idx = 0; finger_idx < 5; finger_idx++) { 222 222 - for (int bone_idx = 0; bone_idx < 5; bone_idx++) { 223 223 - bone_update_quat_and_matrix(&hand->fingers[finger_idx].bones[bone_idx]); 224 224 - } 225 225 - } 226 226 - 227 227 - _statics_init_world_parents(hand); 228 228 - _statics_init_world_poses(hand); 229 229 - _statics_init_loc_ptrs(hand); 230 230 - _statics_joint_limits(hand); 231 231 - } 232 232 - } // namespace xrt::tracking::hand::mercury::ccdik

-31

src/xrt/tracking/hand/mercury/kine_ccdik/ccdik_interface.hpp

reviewed

··· 1 1 - // Copyright 2022, Collabora, Ltd. 2 2 - // SPDX-License-Identifier: BSL-1.0 3 3 - /*! 4 4 - * @file 5 5 - * @brief Interface for Cyclic Coordinate Descent IK kinematic optimizer 6 6 - * @author Moses Turner <moses@collabora.com> 7 7 - * @ingroup tracking 8 8 - */ 9 9 - #pragma once 10 10 - // #include "math/m_api.h" 11 11 - 12 12 - #include "xrt/xrt_defines.h" 13 13 - #include "../kine_common.hpp" 14 14 - 15 15 - namespace xrt::tracking::hand::mercury::ccdik { 16 16 - 17 17 - struct KinematicHandCCDIK; 18 18 - 19 19 - void 20 20 - alloc_kinematic_hand(xrt_pose left_in_right, bool is_right, KinematicHandCCDIK **out_kinematic_hand); 21 21 - 22 22 - void 23 23 - optimize_new_frame(KinematicHandCCDIK *hand, one_frame_input &observation, struct xrt_hand_joint_set &out_set); 24 24 - 25 25 - void 26 26 - init_hardcoded_statics(KinematicHandCCDIK *hand, float size = 0.095864); 27 27 - 28 28 - void 29 29 - free_kinematic_hand(KinematicHandCCDIK **kinematic_hand); 30 30 - 31 31 - } // namespace xrt::tracking::hand::mercury::ccdik

-509

src/xrt/tracking/hand/mercury/kine_ccdik/ccdik_main.cpp

reviewed

··· 1 1 - // Copyright 2022, Collabora, Ltd. 2 2 - // SPDX-License-Identifier: BSL-1.0 3 3 - /*! 4 4 - * @file 5 5 - * @brief Main code for kinematic model 6 6 - * @author Moses Turner <moses@collabora.com> 7 7 - * @ingroup tracking 8 8 - */ 9 9 - 10 10 - #include "ccdik_defines.hpp" 11 11 - #include "ccdik_tiny_math.hpp" 12 12 - #include "ccdik_hand_init.hpp" 13 13 - #include "lineline.hpp" 14 14 - #include "math/m_api.h" 15 15 - #include "util/u_logging.h" 16 16 - 17 17 - #include <Eigen/Core> 18 18 - #include <Eigen/LU> 19 19 - #include <Eigen/SVD> 20 20 - #include <Eigen/src/Geometry/Umeyama.h> 21 21 - 22 22 - 23 23 - 24 24 - namespace xrt::tracking::hand::mercury::ccdik { 25 25 - 26 26 - static void 27 27 - _two_set_ele(Eigen::Matrix<float, 3, kNumNNJoints> &thing, Eigen::Affine3f jt, int idx) 28 28 - { 29 29 - //! @todo Optimize 30 30 - thing.col(idx) = jt.translation(); 31 31 - } 32 32 - 33 33 - static void 34 34 - two(struct KinematicHandCCDIK *hand) 35 35 - { 36 36 - XRT_TRACE_MARKER(); 37 37 - 38 38 - 39 39 - int i = 0; 40 40 - 41 41 - _two_set_ele(hand->kinematic, hand->wrist_relation, i); 42 42 - 43 43 - for (int finger_idx = 0; finger_idx < 5; finger_idx++) { 44 44 - for (int bone_idx = 1; bone_idx < 5; bone_idx++) { 45 45 - 46 46 - i++; 47 47 - 48 48 - _two_set_ele(hand->kinematic, hand->fingers[finger_idx].bones[bone_idx].world_pose, i); 49 49 - } 50 50 - } 51 51 - 52 52 - 53 53 - Eigen::Affine3f aff3d; 54 54 - 55 55 - aff3d = Eigen::umeyama(hand->kinematic, hand->t_jts_as_mat, false); 56 56 - 57 57 - hand->wrist_relation = aff3d * hand->wrist_relation; 58 58 - 59 59 - _statics_init_world_poses(hand); 60 60 - } 61 61 - 62 62 - #if 0 63 63 - #define DOT_EPSILON 0.000001f 64 64 - 65 65 - Eigen::Quaternionf 66 66 - fast_simple_rotation(const Eigen::Vector3f &from_un, const Eigen::Vector3f &to_un) 67 67 - { 68 68 - // Slooowwwwwww.... 69 69 - Eigen::Vector3f from = from_un.normalized(); 70 70 - Eigen::Vector3f to = to_un.normalized(); 71 71 - 72 72 - Eigen::Vector3f axis = from.cross(to); 73 73 - float dot = from.dot(to); 74 74 - 75 75 - if (dot < (-1.0f + DOT_EPSILON)) { 76 76 - return Eigen::Quaternionf(0, 1, 0, 0); 77 77 - }; 78 78 - 79 79 - Eigen::Quaternionf result(axis.x() * 0.5f, axis.y() * 0.5f, axis.z() * 0.5f, (dot + 1.0f) * 0.5f); 80 80 - result.normalize(); 81 81 - return result; 82 82 - } 83 83 - 84 84 - Eigen::Matrix3f 85 85 - moses_fast_simple_rotation(const Eigen::Vector3f &from_un, const Eigen::Vector3f &to_un) 86 86 - { 87 87 - // Slooowwwwwww.... 88 88 - Eigen::Vector3f from = from_un.normalized(); 89 89 - Eigen::Vector3f to = to_un.normalized(); 90 90 - 91 91 - Eigen::Vector3f axis = from.cross(to).normalized(); 92 92 - float angle = acos(from.dot(to)); 93 93 - 94 94 - // U_LOG_E("HERE %f", angle); 95 95 - // std::cout << to << std::endl; 96 96 - // std::cout << from << std::endl; 97 97 - // std::cout << axis << std::endl; 98 98 - // std::cout << angle << std::endl; 99 99 - 100 100 - Eigen::AngleAxisf ax; 101 101 - 102 102 - ax.axis() = axis; 103 103 - ax.angle() = angle; 104 104 - 105 105 - return ax.matrix(); 106 106 - } 107 107 - #endif 108 108 - 109 109 - 110 110 - static void 111 111 - do_it_for_bone(struct KinematicHandCCDIK *hand, int finger_idx, int bone_idx, bool clamp_to_x_axis_rotation) 112 112 - { 113 113 - finger_t *of = &hand->fingers[finger_idx]; 114 114 - bone_t *bone = &hand->fingers[finger_idx].bones[bone_idx]; 115 115 - int num_children = 0; 116 116 - 117 117 - Eigen::Vector3f kine = Eigen::Vector3f::Zero(); 118 118 - Eigen::Vector3f target = Eigen::Vector3f::Zero(); 119 119 - 120 120 - 121 121 - for (int idx = bone_idx + 1; idx < 5; idx++) { 122 122 - num_children++; 123 123 - target += map_vec3(hand->t_jts[of->bones[idx].keypoint_idx_21]); 124 124 - kine += of->bones[idx].world_pose.translation(); 125 125 - } 126 126 - 127 127 - kine *= 1.0f / (float)num_children; 128 128 - target *= 1.0f / (float)num_children; 129 129 - 130 130 - kine = bone->world_pose.inverse() * kine; 131 131 - target = bone->world_pose.inverse() * target; 132 132 - 133 133 - kine.normalize(); 134 134 - target.normalize(); 135 135 - 136 136 - Eigen::Matrix3f rot = Eigen::Quaternionf().setFromTwoVectors(kine, target).matrix(); 137 137 - 138 138 - bone->bone_relation.linear() = bone->bone_relation.linear() * rot; 139 139 - } 140 140 - 141 141 - static void 142 142 - clamp_to_x_axis(struct KinematicHandCCDIK *hand, 143 143 - int finger_idx, 144 144 - int bone_idx, 145 145 - bool clamp_angle = false, 146 146 - float min_angle = 0, 147 147 - float max_angle = 0) 148 148 - { 149 149 - bone_t *bone = &hand->fingers[finger_idx].bones[bone_idx]; 150 150 - // U_LOG_E("before_anything"); 151 151 - 152 152 - // std::cout << bone->bone_relation.linear().matrix() << std::endl; 153 153 - 154 154 - 155 155 - 156 156 - Eigen::Vector3f new_x = bone->bone_relation.linear() * Eigen::Vector3f::UnitX(); 157 157 - Eigen::Matrix3f correction_rot = 158 158 - Eigen::Quaternionf().setFromTwoVectors(new_x.normalized(), Eigen::Vector3f::UnitX()).matrix(); 159 159 - // U_LOG_E("correction"); 160 160 - 161 161 - // std::cout << correction_rot << std::endl; 162 162 - 163 163 - // U_LOG_E("correction times new_x"); 164 164 - // std::cout << correction_rot * new_x << "\n"; 165 165 - 166 166 - // U_LOG_E("before, where does relation point x"); 167 167 - // std::cout << bone->bone_relation.rotation() * Eigen::Vector3f::UnitX() << "\n"; 168 168 - 169 169 - 170 170 - // Weird that we're left-multiplying here, I don't know why. But it works. 171 171 - bone->bone_relation.linear() = correction_rot * bone->bone_relation.linear(); 172 172 - 173 173 - // U_LOG_E("after_correction"); 174 174 - 175 175 - 176 176 - // std::cout << bone->bone_relation.linear() << std::endl; 177 177 - 178 178 - 179 179 - // U_LOG_E("after, where does relation point x"); 180 180 - // std::cout << bone->bone_relation.linear() * Eigen::Vector3f::UnitX() << "\n"; 181 181 - 182 182 - if (clamp_angle) { 183 183 - //! @todo Optimize: get rid of 1 and 2, we only need 0. 184 184 - 185 185 - // signed angle: asin(Cross product of -z and rot*-z X axis. 186 186 - // U_LOG_E("before X clamp"); 187 187 - // std::cout << bone->bone_relation.linear() << "\n"; 188 188 - 189 189 - // auto euler = bone->bone_relation.linear().eulerAngles(0, 1, 2); 190 190 - 191 191 - Eigen::Vector3f cross = 192 192 - (-Eigen::Vector3f::UnitZ()).cross(bone->bone_relation.linear() * (-Eigen::Vector3f::UnitZ())); 193 193 - 194 194 - // std::cout << bone->bone_relation.linear() << "\n"; 195 195 - 196 196 - 197 197 - // U_LOG_E("eluer before clamp: %f %f %f %f %f", min_angle, max_angle, euler(0), euler(1), euler(2)); 198 198 - // U_LOG_E("eluer before clamp: %f %f %f %f %f %f", euler(0), euler(1), euler(2), cross(0), cross(1) , 199 199 - // cross(2)); 200 200 - 201 201 - 202 202 - 203 203 - //! @todo Optimize: Move the asin into constexpr land 204 204 - // No, the sine of the joint limit 205 205 - float rotation_value = asin(cross(0)); 206 206 - 207 207 - //!@bug No. If the rotation value is outside of the allowed values, clamp it to the rotation it's 208 208 - //!*closest to*. 209 209 - // That's different than using clamp, rotation formalisms are complicated. 210 210 - clamp(&rotation_value, min_angle, max_angle); 211 211 - 212 212 - // U_LOG_E("eluer after clamp: %f", rotation_value); 213 213 - 214 214 - 215 215 - Eigen::Matrix3f n; 216 216 - n = Eigen::AngleAxisf(rotation_value, Eigen::Vector3f::UnitX()); 217 217 - bone->bone_relation.linear() = n; 218 218 - // U_LOG_E("after X clamp"); 219 219 - 220 220 - // std::cout << n << "\n"; 221 221 - } 222 222 - } 223 223 - 224 224 - // Is this not just swing-twist about the Z axis? Dunnoooo... Find out later. 225 225 - static void 226 226 - clamp_proximals(struct KinematicHandCCDIK *hand, 227 227 - int finger_idx, 228 228 - int bone_idx, 229 229 - float max_swing_angle = 0, 230 230 - float tanangle_left = tan(rad(-20)), 231 231 - float tanangle_right = tan(rad(20)), 232 232 - float tanangle_curled = tan(rad(-89)), // Uh oh... 233 233 - float tanangle_uncurled = tan(rad(30))) 234 234 - { 235 235 - bone_t *bone = &hand->fingers[finger_idx].bones[bone_idx]; 236 236 - 237 237 - 238 238 - Eigen::Matrix3f rot = bone->bone_relation.linear(); 239 239 - 240 240 - // U_LOG_E("rot"); 241 241 - // std::cout << rot << "\n"; 242 242 - 243 243 - Eigen::Vector3f our_z = rot * (-Eigen::Vector3f::UnitZ()); 244 244 - 245 245 - Eigen::Matrix3f simple = Eigen::Quaternionf().setFromTwoVectors(-Eigen::Vector3f::UnitZ(), our_z).matrix(); 246 246 - // U_LOG_E("simple"); 247 247 - // std::cout << simple << "\n"; 248 248 - 249 249 - Eigen::Matrix3f twist = rot * simple.inverse(); 250 250 - // U_LOG_E("twist"); 251 251 - 252 252 - // std::cout << twist << "\n"; 253 253 - 254 254 - // U_LOG_E("twist_axis"); 255 255 - 256 256 - Eigen::AngleAxisf twist_aax = Eigen::AngleAxisf(twist); 257 257 - 258 258 - // std::cout << twist_aax.axis() << "\n"; 259 259 - 260 260 - // U_LOG_E("twist_angle"); 261 261 - 262 262 - // std::cout << twist_aax.angle() << "\n"; 263 263 - 264 264 - 265 265 - 266 266 - // U_LOG_E("all together now"); 267 267 - 268 268 - // std::cout << twist * simple << "\n"; 269 269 - 270 270 - if (fabs(twist_aax.angle()) > max_swing_angle) { 271 271 - // max_swing_angle times +1 or -1, depending. 272 272 - twist_aax.angle() = max_swing_angle * (twist_aax.angle() / fabs(twist_aax.angle())); 273 273 - // U_LOG_E("clamping twist %f", twist_aax.angle()); 274 274 - 275 275 - 276 276 - // std::cout << twist << "\n"; 277 277 - // std::cout << twist_aax.toRotationMatrix() << "\n"; 278 278 - } 279 279 - 280 280 - 281 281 - if (our_z.z() > 0) { 282 282 - //!@bug We need smarter joint limiting, limiting via tanangles is not acceptable as joints can rotate 283 283 - //! outside of the 180 degree hemisphere. 284 284 - our_z.z() = -0.000001f; 285 285 - } 286 286 - our_z *= -1.0f / our_z.z(); 287 287 - 288 288 - 289 289 - clamp(&our_z.x(), tanangle_left, tanangle_right); 290 290 - clamp(&our_z.y(), tanangle_curled, tanangle_uncurled); 291 291 - 292 292 - simple = Eigen::Quaternionf().setFromTwoVectors(-Eigen::Vector3f::UnitZ(), our_z.normalized()).matrix(); 293 293 - 294 294 - bone->bone_relation.linear() = twist_aax * simple; 295 295 - } 296 296 - 297 297 - 298 298 - 299 299 - static void 300 300 - do_it_for_finger(struct KinematicHandCCDIK *hand, int finger_idx) 301 301 - { 302 302 - do_it_for_bone(hand, finger_idx, 0, false); 303 303 - clamp_proximals(hand, finger_idx, 0, rad(4.0), tan(rad(-30)), tan(rad(30)), tan(rad(-10)), tan(rad(10))); 304 304 - _statics_init_world_poses(hand); 305 305 - 306 306 - do_it_for_bone(hand, finger_idx, 1, true); 307 307 - clamp_proximals(hand, finger_idx, 1, rad(4.0)); 308 308 - _statics_init_world_poses(hand); 309 309 - 310 310 - 311 311 - do_it_for_bone(hand, finger_idx, 2, true); 312 312 - clamp_to_x_axis(hand, finger_idx, 2, true, rad(-90), rad(10)); 313 313 - _statics_init_world_poses(hand); 314 314 - 315 315 - do_it_for_bone(hand, finger_idx, 3, true); 316 316 - clamp_to_x_axis(hand, finger_idx, 3, true, rad(-90), rad(10)); 317 317 - _statics_init_world_poses(hand); 318 318 - } 319 319 - 320 320 - static void 321 321 - optimize(KinematicHandCCDIK *hand) 322 322 - { 323 323 - for (int i = 0; i < 15; i++) { 324 324 - two(hand); 325 325 - do_it_for_bone(hand, 0, 1, false); 326 326 - clamp_proximals(hand, 0, 1, rad(70), tan(rad(-40)), tan(rad(40)), tan(rad(-40)), tan(rad(40))); 327 327 - _statics_init_world_poses(hand); 328 328 - 329 329 - do_it_for_bone(hand, 0, 2, true); 330 330 - clamp_to_x_axis(hand, 0, 2, true, rad(-90), rad(40)); 331 331 - _statics_init_world_poses(hand); 332 332 - 333 333 - do_it_for_bone(hand, 0, 3, true); 334 334 - clamp_to_x_axis(hand, 0, 3, true, rad(-90), rad(40)); 335 335 - _statics_init_world_poses(hand); 336 336 - 337 337 - two(hand); 338 338 - 339 339 - do_it_for_finger(hand, 1); 340 340 - do_it_for_finger(hand, 2); 341 341 - do_it_for_finger(hand, 3); 342 342 - do_it_for_finger(hand, 4); 343 343 - } 344 344 - two(hand); 345 345 - } 346 346 - 347 347 - 348 348 - static void 349 349 - make_joint_at_matrix_left_hand(int idx, Eigen::Affine3f &pose, struct xrt_hand_joint_set &hand) 350 350 - { 351 351 - hand.values.hand_joint_set_default[idx].relation.relation_flags = (enum xrt_space_relation_flags)( 352 352 - XRT_SPACE_RELATION_ORIENTATION_VALID_BIT | XRT_SPACE_RELATION_ORIENTATION_TRACKED_BIT | 353 353 - XRT_SPACE_RELATION_POSITION_VALID_BIT | XRT_SPACE_RELATION_POSITION_TRACKED_BIT); 354 354 - Eigen::Vector3f v = pose.translation(); 355 355 - hand.values.hand_joint_set_default[idx].relation.pose.position.x = v.x(); 356 356 - hand.values.hand_joint_set_default[idx].relation.pose.position.y = v.y(); 357 357 - hand.values.hand_joint_set_default[idx].relation.pose.position.z = v.z(); 358 358 - 359 359 - Eigen::Quaternionf q; 360 360 - q = pose.rotation(); 361 361 - 362 362 - hand.values.hand_joint_set_default[idx].relation.pose.orientation.x = q.x(); 363 363 - hand.values.hand_joint_set_default[idx].relation.pose.orientation.y = q.y(); 364 364 - hand.values.hand_joint_set_default[idx].relation.pose.orientation.z = q.z(); 365 365 - hand.values.hand_joint_set_default[idx].relation.pose.orientation.w = q.w(); 366 366 - } 367 367 - 368 368 - static void 369 369 - make_joint_at_matrix_right_hand(int idx, Eigen::Affine3f &pose, struct xrt_hand_joint_set &hand) 370 370 - { 371 371 - hand.values.hand_joint_set_default[idx].relation.relation_flags = (enum xrt_space_relation_flags)( 372 372 - XRT_SPACE_RELATION_ORIENTATION_VALID_BIT | XRT_SPACE_RELATION_ORIENTATION_TRACKED_BIT | 373 373 - XRT_SPACE_RELATION_POSITION_VALID_BIT | XRT_SPACE_RELATION_POSITION_TRACKED_BIT); 374 374 - Eigen::Vector3f v = pose.translation(); 375 375 - hand.values.hand_joint_set_default[idx].relation.pose.position.x = -v.x(); 376 376 - hand.values.hand_joint_set_default[idx].relation.pose.position.y = v.y(); 377 377 - hand.values.hand_joint_set_default[idx].relation.pose.position.z = v.z(); 378 378 - 379 379 - Eigen::Matrix3f rotation = pose.rotation(); 380 380 - 381 381 - Eigen::Matrix3f mirror_on_x = Eigen::Matrix3f::Identity(); 382 382 - mirror_on_x(0, 0) = -1; 383 383 - 384 384 - rotation = mirror_on_x * rotation; 385 385 - 386 386 - rotation(0, 0) *= -1; 387 387 - rotation(1, 0) *= -1; 388 388 - rotation(2, 0) *= -1; 389 389 - 390 390 - Eigen::Quaternionf q; 391 391 - 392 392 - q = rotation; 393 393 - 394 394 - hand.values.hand_joint_set_default[idx].relation.pose.orientation.x = q.x(); 395 395 - hand.values.hand_joint_set_default[idx].relation.pose.orientation.y = q.y(); 396 396 - hand.values.hand_joint_set_default[idx].relation.pose.orientation.z = q.z(); 397 397 - hand.values.hand_joint_set_default[idx].relation.pose.orientation.w = q.w(); 398 398 - } 399 399 - 400 400 - static void 401 401 - make_joint_at_matrix(int idx, Eigen::Affine3f &pose, struct xrt_hand_joint_set &hand, bool is_right) 402 402 - { 403 403 - if (!is_right) { 404 404 - make_joint_at_matrix_left_hand(idx, pose, hand); 405 405 - } else { 406 406 - make_joint_at_matrix_right_hand(idx, pose, hand); 407 407 - } 408 408 - } 409 409 - 410 410 - // Exported: 411 411 - void 412 412 - optimize_new_frame(KinematicHandCCDIK *hand, one_frame_input &observation, struct xrt_hand_joint_set &out_set) 413 413 - { 414 414 - 415 415 - // intake poses! 416 416 - for (int i = 0; i < 21; i++) { 417 417 - 418 418 - xrt_vec3 p1 = {0, 0, 0}; 419 419 - xrt_vec3 p2 = observation.views[0].rays[i]; 420 420 - 421 421 - xrt_vec3 p3 = hand->right_in_left.position; 422 422 - 423 423 - xrt_vec3 p4; 424 424 - math_quat_rotate_vec3(&hand->right_in_left.orientation, &observation.views[1].rays[i], &p4); 425 425 - p4 += hand->right_in_left.position; 426 426 - 427 427 - xrt_vec3 pa; 428 428 - xrt_vec3 pb; 429 429 - float mua; 430 430 - float mub; 431 431 - 432 432 - LineLineIntersect(p1, p2, p3, p4, &pa, &pb, &mua, &mub); 433 433 - 434 434 - xrt_vec3 p; 435 435 - p = pa + pb; 436 436 - math_vec3_scalar_mul(0.5, &p); 437 437 - 438 438 - 439 439 - if (!hand->is_right) { 440 440 - 441 441 - hand->t_jts[i] = p; 442 442 - } else { 443 443 - hand->t_jts[i].x = -p.x; 444 444 - hand->t_jts[i].y = p.y; 445 445 - hand->t_jts[i].z = p.z; 446 446 - } 447 447 - 448 448 - hand->t_jts_as_mat(0, i) = hand->t_jts[i].x; 449 449 - hand->t_jts_as_mat(1, i) = hand->t_jts[i].y; 450 450 - hand->t_jts_as_mat(2, i) = hand->t_jts[i].z; 451 451 - } 452 452 - 453 453 - // do the math! 454 454 - optimize(hand); 455 455 - 456 456 - // Convert it to xrt_hand_joint_set! 457 457 - 458 458 - make_joint_at_matrix(XRT_HAND_JOINT_WRIST, hand->wrist_relation, out_set, hand->is_right); 459 459 - 460 460 - Eigen::Affine3f palm_relation; 461 461 - 462 462 - palm_relation.linear() = hand->fingers[2].bones[0].world_pose.linear(); 463 463 - 464 464 - palm_relation.translation() = Eigen::Vector3f::Zero(); 465 465 - palm_relation.translation() += hand->fingers[2].bones[0].world_pose.translation() / 2; 466 466 - palm_relation.translation() += hand->fingers[2].bones[1].world_pose.translation() / 2; 467 467 - 468 468 - make_joint_at_matrix(XRT_HAND_JOINT_PALM, palm_relation, out_set, hand->is_right); 469 469 - 470 470 - int start = XRT_HAND_JOINT_THUMB_METACARPAL; 471 471 - 472 472 - for (int finger_idx = 0; finger_idx < 5; finger_idx++) { 473 473 - 474 474 - for (int bone_idx = 0; bone_idx < 5; bone_idx++) { 475 475 - CONTINUE_IF_HIDDEN_THUMB; 476 476 - 477 477 - if (!(finger_idx == 0 && bone_idx == 0)) { 478 478 - make_joint_at_matrix(start++, hand->fingers[finger_idx].bones[bone_idx].world_pose, 479 479 - out_set, hand->is_right); 480 480 - } 481 481 - } 482 482 - } 483 483 - 484 484 - out_set.is_active = true; 485 485 - } 486 486 - 487 487 - void 488 488 - alloc_kinematic_hand(xrt_pose left_in_right, bool is_right, KinematicHandCCDIK **out_kinematic_hand) 489 489 - { 490 490 - KinematicHandCCDIK *h = new KinematicHandCCDIK(); 491 491 - h->is_right = is_right; 492 492 - 493 493 - math_pose_invert(&left_in_right, &h->right_in_left); 494 494 - 495 495 - // U_LOG_E("%f %f %f", h->right_in_left.position.x, h->right_in_left.position.y, h->right_in_left.position.z); 496 496 - 497 497 - // Doesn't matter, should get overwritten later. 498 498 - init_hardcoded_statics(h, 0.09f); 499 499 - 500 500 - *out_kinematic_hand = h; 501 501 - } 502 502 - 503 503 - void 504 504 - free_kinematic_hand(KinematicHandCCDIK **kinematic_hand) 505 505 - { 506 506 - delete *kinematic_hand; 507 507 - } 508 508 - 509 509 - } // namespace xrt::tracking::hand::mercury::ccdik

-57

src/xrt/tracking/hand/mercury/kine_ccdik/ccdik_tiny_math.hpp

reviewed

··· 1 1 - // Copyright 2022, Collabora, Ltd. 2 2 - // SPDX-License-Identifier: BSL-1.0 3 3 - /*! 4 4 - * @file 5 5 - * @brief Math for kinematic model 6 6 - * @author Moses Turner <moses@collabora.com> 7 7 - * @ingroup tracking 8 8 - */ 9 9 - 10 10 - #pragma once 11 11 - #include "ccdik_defines.hpp" 12 12 - 13 13 - namespace xrt::tracking::hand::mercury::ccdik { 14 14 - // Waggle-curl-twist. 15 15 - static inline void 16 16 - wct_to_quat(wct_t wct, struct xrt_quat *out) 17 17 - { 18 18 - XRT_TRACE_MARKER(); 19 19 - xrt_vec3 waggle_axis = {0, 1, 0}; 20 20 - xrt_quat just_waggle; 21 21 - math_quat_from_angle_vector(wct.waggle, &waggle_axis, &just_waggle); 22 22 - 23 23 - xrt_vec3 curl_axis = {1, 0, 0}; 24 24 - xrt_quat just_curl; 25 25 - math_quat_from_angle_vector(wct.curl, &curl_axis, &just_curl); 26 26 - 27 27 - xrt_vec3 twist_axis = {0, 0, 1}; 28 28 - xrt_quat just_twist; 29 29 - math_quat_from_angle_vector(wct.twist, &twist_axis, &just_twist); 30 30 - 31 31 - //! @todo Optimize This should be a matrix multiplication... 32 32 - // Are you sure about that, previous moses? Pretty sure that quat products are faster than 3x3 matrix 33 33 - // products... 34 34 - *out = just_waggle; 35 35 - math_quat_rotate(out, &just_curl, out); 36 36 - math_quat_rotate(out, &just_twist, out); 37 37 - } 38 38 - 39 39 - // Inlines. 40 40 - static inline float 41 41 - rad(double degrees) 42 42 - { 43 43 - return degrees * (M_PI / 180.0); 44 44 - } 45 45 - 46 46 - static inline void 47 47 - clamp(float *in, float min, float max) 48 48 - { 49 49 - *in = fminf(max, fmaxf(min, *in)); 50 50 - } 51 51 - 52 52 - static inline void 53 53 - clamp_to_r(float *in, float c, float r) 54 54 - { 55 55 - clamp(in, c - r, c + r); 56 56 - } 57 57 - } // namespace xrt::tracking::hand::mercury::ccdik

-89

src/xrt/tracking/hand/mercury/kine_ccdik/lineline.hpp

reviewed

··· 1 1 - // Copyright 1998, Paul Bourke. 2 2 - // Copyright 2022, Collabora, Ltd. 3 3 - // SPDX-License-Identifier: BSL-1.0 4 4 - /*! 5 5 - * @file 6 6 - * @brief Find the closest approach between two lines 7 7 - * @author Paul Bourke <paul.bourke@gmail.com> 8 8 - * @author Moses Turner <moses@collabora.com> 9 9 - * @ingroup tracking 10 10 - */ 11 11 - #include "float.h" 12 12 - #include <limits.h> 13 13 - #include <math.h> 14 14 - #include "util/u_logging.h" 15 15 - #include "xrt/xrt_defines.h" 16 16 - #include "stdbool.h" 17 17 - 18 18 - // Taken and relicensed with written permission from http://paulbourke.net/geometry/pointlineplane/lineline.c + 19 19 - // http://paulbourke.net/geometry/pointlineplane/ 20 20 - 21 21 - /* 22 22 - Calculate the line segment PaPb that is the shortest route between 23 23 - two lines P1P2 and P3P4. Calculate also the values of mua and mub where 24 24 - Pa = P1 + mua (P2 - P1) 25 25 - Pb = P3 + mub (P4 - P3) 26 26 - Return false if no solution exists. 27 27 - */ 28 28 - bool 29 29 - LineLineIntersect(struct xrt_vec3 p1, 30 30 - struct xrt_vec3 p2, 31 31 - struct xrt_vec3 p3, 32 32 - struct xrt_vec3 p4, 33 33 - struct xrt_vec3 *pa, 34 34 - struct xrt_vec3 *pb, 35 35 - float *mua, 36 36 - float *mub) 37 37 - { 38 38 - struct xrt_vec3 p13, p43, p21; // NOLINT 39 39 - float d1343, d4321, d1321, d4343, d2121; // NOLINT 40 40 - float number, denom; // NOLINT 41 41 - 42 42 - p13.x = p1.x - p3.x; 43 43 - p13.y = p1.y - p3.y; 44 44 - p13.z = p1.z - p3.z; 45 45 - 46 46 - p43.x = p4.x - p3.x; 47 47 - p43.y = p4.y - p3.y; 48 48 - p43.z = p4.z - p3.z; 49 49 - // Disabled - just checks that P3P4 isn't length 0, which it won't be. 50 50 - #if 0 51 51 - if (ABS(p43.x) < FLT_EPSILON && ABS(p43.y) < FLT_EPSILON && ABS(p43.z) < FLT_EPSILON) 52 52 - return false; 53 53 - #endif 54 54 - p21.x = p2.x - p1.x; 55 55 - p21.y = p2.y - p1.y; 56 56 - p21.z = p2.z - p1.z; 57 57 - 58 58 - // Ditto, checks that P2P1 isn't length 0. 59 59 - #if 0 60 60 - if (ABS(p21.x) < EPS && ABS(p21.y) < EPS && ABS(p21.z) < EPS) 61 61 - return false; 62 62 - #endif 63 63 - 64 64 - d1343 = p13.x * p43.x + p13.y * p43.y + p13.z * p43.z; 65 65 - d4321 = p43.x * p21.x + p43.y * p21.y + p43.z * p21.z; 66 66 - d1321 = p13.x * p21.x + p13.y * p21.y + p13.z * p21.z; 67 67 - d4343 = p43.x * p43.x + p43.y * p43.y + p43.z * p43.z; 68 68 - d2121 = p21.x * p21.x + p21.y * p21.y + p21.z * p21.z; 69 69 - 70 70 - denom = d2121 * d4343 - d4321 * d4321; 71 71 - 72 72 - // Division-by-zero check 73 73 - if (fabsf(denom) < FLT_EPSILON) { 74 74 - return false; 75 75 - } 76 76 - number = d1343 * d4321 - d1321 * d4343; 77 77 - 78 78 - *mua = number / denom; 79 79 - *mub = (d1343 + d4321 * (*mua)) / d4343; 80 80 - 81 81 - pa->x = p1.x + *mua * p21.x; 82 82 - pa->y = p1.y + *mua * p21.y; 83 83 - pa->z = p1.z + *mua * p21.z; 84 84 - pb->x = p3.x + *mub * p43.x; 85 85 - pb->y = p3.y + *mub * p43.y; 86 86 - pb->z = p3.z + *mub * p43.z; 87 87 - 88 88 - return (true); 89 89 - }