mirror of
https://gitlab.freedesktop.org/monado/monado.git
synced 2025-02-15 10:10:07 +00:00
h/mercury: Remove kine_ccdik optimizer
Goodbye, sweet prince. This was my first attempt at the "optimizer" piece of our optical hand tracking, and it *did work* dammit! It just wasn't anywhere near as flexible or efficient as Levenberg-Marquardt. It's worse in every way to the `kine_lm` optimizer, and getting hard to maintain, so we're getting rid of it. Gone, but never forgotten.
This commit is contained in:
parent
7da72f4697
commit
946141ce40
src/xrt/tracking/hand/mercury
|
@ -4,7 +4,6 @@
|
|||
# Mercury hand tracking library!
|
||||
|
||||
add_subdirectory(kine_lm)
|
||||
add_subdirectory(kine_ccdik)
|
||||
|
||||
add_library(t_ht_mercury_model STATIC hg_model.cpp)
|
||||
|
||||
|
@ -37,7 +36,6 @@ target_link_libraries(
|
|||
aux_util
|
||||
ONNXRuntime::ONNXRuntime
|
||||
t_ht_mercury_kine_lm
|
||||
t_ht_mercury_kine_ccdik
|
||||
t_ht_mercury_model
|
||||
# ncnn # Soon...
|
||||
${OpenCV_LIBRARIES}
|
||||
|
|
|
@ -664,9 +664,6 @@ HandTracking::~HandTracking()
|
|||
lm::optimizer_destroy(&this->kinematic_hands[0]);
|
||||
lm::optimizer_destroy(&this->kinematic_hands[1]);
|
||||
|
||||
ccdik::free_kinematic_hand(&this->kinematic_hands_ccdik[0]);
|
||||
ccdik::free_kinematic_hand(&this->kinematic_hands_ccdik[1]);
|
||||
|
||||
u_var_remove_root((void *)&this->base);
|
||||
u_frame_times_widget_teardown(&this->ft_widget);
|
||||
}
|
||||
|
@ -886,50 +883,39 @@ HandTracking::cCallbackProcess(struct t_hand_tracking_sync *ht_sync,
|
|||
|
||||
struct xrt_hand_joint_set *put_in_set = out_xrt_hands[hand_idx];
|
||||
|
||||
if (__builtin_expect(!hgt->tuneable_values.use_ccdik, true)) {
|
||||
lm::KinematicHandLM *hand = hgt->kinematic_hands[hand_idx];
|
||||
lm::KinematicHandLM *hand = hgt->kinematic_hands[hand_idx];
|
||||
|
||||
//!@todo
|
||||
// ABOUT TWO MINUTES OF THOUGHT WERE PUT INTO THIS VALUE
|
||||
float reprojection_error_threshold = 0.35f;
|
||||
//!@todo
|
||||
// ABOUT TWO MINUTES OF THOUGHT WERE PUT INTO THIS VALUE
|
||||
float reprojection_error_threshold = 0.35f;
|
||||
|
||||
float out_hand_size;
|
||||
float out_hand_size;
|
||||
|
||||
//!@todo Optimize: We can have one of these on each thread
|
||||
float reprojection_error;
|
||||
lm::optimizer_run(hand, //
|
||||
input, //
|
||||
!hgt->last_frame_hand_detected[hand_idx], //
|
||||
optimize_hand_size, //
|
||||
hgt->target_hand_size, //
|
||||
hgt->refinement.hand_size_refinement_schedule_y, //
|
||||
*put_in_set, //
|
||||
out_hand_size, //
|
||||
reprojection_error);
|
||||
//!@todo Optimize: We can have one of these on each thread
|
||||
float reprojection_error;
|
||||
lm::optimizer_run(hand, //
|
||||
input, //
|
||||
!hgt->last_frame_hand_detected[hand_idx], //
|
||||
optimize_hand_size, //
|
||||
hgt->target_hand_size, //
|
||||
hgt->refinement.hand_size_refinement_schedule_y, //
|
||||
*put_in_set, //
|
||||
out_hand_size, //
|
||||
reprojection_error);
|
||||
|
||||
avg_hand_size += out_hand_size;
|
||||
num_hands++;
|
||||
avg_hand_size += out_hand_size;
|
||||
num_hands++;
|
||||
|
||||
if (reprojection_error > reprojection_error_threshold) {
|
||||
HG_DEBUG(hgt, "Reprojection error above threshold!");
|
||||
hgt->this_frame_hand_detected[hand_idx] = false;
|
||||
if (reprojection_error > reprojection_error_threshold) {
|
||||
HG_DEBUG(hgt, "Reprojection error above threshold!");
|
||||
hgt->this_frame_hand_detected[hand_idx] = false;
|
||||
|
||||
continue;
|
||||
}
|
||||
if (!any_hands_are_only_visible_in_one_view) {
|
||||
hgt->refinement.hand_size_refinement_schedule_x +=
|
||||
hand_confidence_value(reprojection_error, input);
|
||||
}
|
||||
|
||||
} else {
|
||||
ccdik::KinematicHandCCDIK *hand = hgt->kinematic_hands_ccdik[hand_idx];
|
||||
if (!hgt->last_frame_hand_detected[hand_idx]) {
|
||||
ccdik::init_hardcoded_statics(hand, hgt->target_hand_size);
|
||||
}
|
||||
ccdik::optimize_new_frame(hand, input, *put_in_set);
|
||||
continue;
|
||||
}
|
||||
if (!any_hands_are_only_visible_in_one_view) {
|
||||
hgt->refinement.hand_size_refinement_schedule_x +=
|
||||
hand_confidence_value(reprojection_error, input);
|
||||
}
|
||||
|
||||
|
||||
|
||||
u_hand_joints_apply_joint_width(put_in_set);
|
||||
|
||||
|
@ -1053,9 +1039,6 @@ t_hand_tracking_sync_mercury_create(struct t_stereo_camera_calibration *calib,
|
|||
lm::optimizer_create(hgt->left_in_right, false, hgt->log_level, &hgt->kinematic_hands[0]);
|
||||
lm::optimizer_create(hgt->left_in_right, true, hgt->log_level, &hgt->kinematic_hands[1]);
|
||||
|
||||
ccdik::alloc_kinematic_hand(hgt->left_in_right, false, &hgt->kinematic_hands_ccdik[0]);
|
||||
ccdik::alloc_kinematic_hand(hgt->left_in_right, true, &hgt->kinematic_hands_ccdik[1]);
|
||||
|
||||
u_frame_times_widget_init(&hgt->ft_widget, 10.0f, 10.0f);
|
||||
|
||||
u_var_add_root(hgt, "Camera-based Hand Tracker", true);
|
||||
|
@ -1100,8 +1083,6 @@ t_hand_tracking_sync_mercury_create(struct t_stereo_camera_calibration *calib,
|
|||
u_var_add_bool(hgt, &hgt->tuneable_values.scribble_keypoint_model_outputs, "Scribble keypoint model output");
|
||||
u_var_add_bool(hgt, &hgt->tuneable_values.scribble_optimizer_outputs, "Scribble kinematic optimizer output");
|
||||
u_var_add_bool(hgt, &hgt->tuneable_values.always_run_detection_model, "Always run detection model");
|
||||
u_var_add_bool(hgt, &hgt->tuneable_values.use_ccdik,
|
||||
"Use IK optimizer (may put tracking in unexpected state, use with care)");
|
||||
|
||||
|
||||
u_var_add_sink_debug(hgt, &hgt->debug_sink_ann, "Annotated camera feeds");
|
||||
|
|
|
@ -46,7 +46,6 @@
|
|||
|
||||
#include "kine_common.hpp"
|
||||
#include "kine_lm/lm_interface.hpp"
|
||||
#include "kine_ccdik/ccdik_interface.hpp"
|
||||
|
||||
|
||||
|
||||
|
@ -240,7 +239,6 @@ public:
|
|||
enum u_logging_level log_level = U_LOGGING_INFO;
|
||||
|
||||
lm::KinematicHandLM *kinematic_hands[2];
|
||||
ccdik::KinematicHandCCDIK *kinematic_hands_ccdik[2];
|
||||
|
||||
// struct hand_detection_state_tracker st_det[2] = {};
|
||||
bool hand_seen_before[2] = {false, false};
|
||||
|
@ -277,7 +275,6 @@ public:
|
|||
bool scribble_keypoint_model_outputs = false;
|
||||
bool scribble_optimizer_outputs = true;
|
||||
bool always_run_detection_model = false;
|
||||
bool use_ccdik = false;
|
||||
int max_num_outside_view = 3;
|
||||
} tuneable_values;
|
||||
|
||||
|
|
|
@ -1,8 +0,0 @@
|
|||
# Copyright 2022, Collabora, Ltd.
|
||||
# SPDX-License-Identifier: BSL-1.0
|
||||
|
||||
add_library(t_ht_mercury_kine_ccdik STATIC ccdik_interface.hpp ccdik_main.cpp)
|
||||
|
||||
target_link_libraries(t_ht_mercury_kine_ccdik PRIVATE aux_math aux_tracking aux_os aux_util)
|
||||
|
||||
target_include_directories(t_ht_mercury_kine_ccdik SYSTEM PRIVATE ${EIGEN3_INCLUDE_DIR})
|
|
@ -1,178 +0,0 @@
|
|||
// Copyright 2022, Collabora, Ltd.
|
||||
// SPDX-License-Identifier: BSL-1.0
|
||||
/*!
|
||||
* @file
|
||||
* @brief Defines for kinematic model
|
||||
* @author Moses Turner <moses@collabora.com>
|
||||
* @ingroup tracking
|
||||
*/
|
||||
|
||||
|
||||
#pragma once
|
||||
#include "math/m_api.h"
|
||||
|
||||
|
||||
#include <stddef.h>
|
||||
#include <unistd.h>
|
||||
|
||||
#include <Eigen/Core>
|
||||
#include <Eigen/Geometry>
|
||||
|
||||
#include "math/m_eigen_interop.hpp"
|
||||
#include "math/m_api.h"
|
||||
#include "math/m_space.h"
|
||||
#include "math/m_vec3.h"
|
||||
|
||||
#include "util/u_trace_marker.h"
|
||||
|
||||
#include "os/os_time.h"
|
||||
#include "util/u_time.h"
|
||||
|
||||
#include <iostream>
|
||||
#include <vector>
|
||||
|
||||
#include <stdio.h>
|
||||
|
||||
#include <stddef.h>
|
||||
#include <unistd.h>
|
||||
#include "../kine_common.hpp"
|
||||
|
||||
|
||||
using namespace xrt::auxiliary::math;
|
||||
|
||||
namespace xrt::tracking::hand::mercury::ccdik {
|
||||
|
||||
enum class HandJoint26KP : int
|
||||
{
|
||||
PALM = 0,
|
||||
WRIST = 1,
|
||||
|
||||
THUMB_METACARPAL = 2,
|
||||
THUMB_PROXIMAL = 3,
|
||||
THUMB_DISTAL = 4,
|
||||
THUMB_TIP = 5,
|
||||
|
||||
INDEX_METACARPAL = 6,
|
||||
INDEX_PROXIMAL = 7,
|
||||
INDEX_INTERMEDIATE = 8,
|
||||
INDEX_DISTAL = 9,
|
||||
INDEX_TIP = 10,
|
||||
|
||||
MIDDLE_METACARPAL = 11,
|
||||
MIDDLE_PROXIMAL = 12,
|
||||
MIDDLE_INTERMEDIATE = 13,
|
||||
MIDDLE_DISTAL = 14,
|
||||
MIDDLE_TIP = 15,
|
||||
|
||||
RING_METACARPAL = 16,
|
||||
RING_PROXIMAL = 17,
|
||||
RING_INTERMEDIATE = 18,
|
||||
RING_DISTAL = 19,
|
||||
RING_TIP = 20,
|
||||
|
||||
LITTLE_METACARPAL = 21,
|
||||
LITTLE_PROXIMAL = 22,
|
||||
LITTLE_INTERMEDIATE = 23,
|
||||
LITTLE_DISTAL = 24,
|
||||
LITTLE_TIP = 25,
|
||||
};
|
||||
|
||||
enum HandFinger
|
||||
{
|
||||
HF_THUMB = 0,
|
||||
HF_INDEX = 1,
|
||||
HF_MIDDLE = 2,
|
||||
HF_RING = 3,
|
||||
HF_LITTLE = 4,
|
||||
};
|
||||
|
||||
enum FingerBone
|
||||
{
|
||||
// FB_CARPAL,
|
||||
FB_METACARPAL,
|
||||
FB_PROXIMAL,
|
||||
FB_INTERMEDIATE,
|
||||
FB_DISTAL
|
||||
};
|
||||
|
||||
enum ThumbBone
|
||||
{
|
||||
// TB_CARPAL,
|
||||
TB_METACARPAL,
|
||||
TB_PROXIMAL,
|
||||
TB_DISTAL
|
||||
};
|
||||
|
||||
const size_t kNumNNJoints = 21;
|
||||
|
||||
struct wct_t
|
||||
{
|
||||
float waggle = 0.0f;
|
||||
float curl = 0.0f;
|
||||
float twist = 0.0f;
|
||||
};
|
||||
|
||||
// IGNORE THE FIRST BONE in the wrist.
|
||||
struct bone_t
|
||||
{
|
||||
// EIGEN_OVERRIDE_OPERATOR_NEW
|
||||
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
|
||||
// will always be 0, 0, -(some amount) for mcp, pxm, int, dst
|
||||
// will be random amounts for carpal bones
|
||||
Eigen::Vector3f trans_from_last_joint = Eigen::Vector3f::Zero();
|
||||
wct_t rot_to_next_joint_wct = {};
|
||||
Eigen::Quaternionf rot_to_next_joint_quat = {};
|
||||
// Translation from last joint to this joint, rotation that takes last joint's -z and points it at next joint
|
||||
Eigen::Affine3f bone_relation = {};
|
||||
|
||||
// Imagine it like transforming an object at the origin to this bone's position/orientation.
|
||||
Eigen::Affine3f world_pose = {};
|
||||
|
||||
struct
|
||||
{
|
||||
Eigen::Affine3f *world_pose;
|
||||
Eigen::Affine3f *bone_relation;
|
||||
} parent;
|
||||
|
||||
|
||||
wct_t joint_limit_min = {};
|
||||
wct_t joint_limit_max = {};
|
||||
|
||||
|
||||
// What keypoint out of the ML model does this correspond to?
|
||||
Joint21::Joint21 keypoint_idx_21 = {};
|
||||
};
|
||||
|
||||
// translation: wrist to mcp (-z and x). rotation: from wrist space to metacarpal space
|
||||
// translation: mcp to pxm (just -z). rotation: from mcp to pxm space
|
||||
|
||||
struct finger_t
|
||||
{
|
||||
bone_t bones[5] = {};
|
||||
};
|
||||
|
||||
|
||||
struct KinematicHandCCDIK
|
||||
{
|
||||
// The distance from the wrist to the middle-proximal joint - this sets the overall hand size.
|
||||
float size;
|
||||
bool is_right;
|
||||
xrt_pose right_in_left;
|
||||
|
||||
// Wrist pose, scaled by size.
|
||||
Eigen::Affine3f wrist_relation = {};
|
||||
|
||||
finger_t fingers[5] = {};
|
||||
|
||||
xrt_vec3 t_jts[kNumNNJoints] = {};
|
||||
Eigen::Matrix<float, 3, kNumNNJoints> t_jts_as_mat = {};
|
||||
Eigen::Matrix<float, 3, kNumNNJoints> kinematic = {};
|
||||
};
|
||||
|
||||
|
||||
#define CONTINUE_IF_HIDDEN_THUMB \
|
||||
if (finger_idx == 0 && bone_idx == 0) { \
|
||||
continue; \
|
||||
}
|
||||
|
||||
} // namespace xrt::tracking::hand::mercury::ccdik
|
|
@ -1,232 +0,0 @@
|
|||
// Copyright 2022, Collabora, Ltd.
|
||||
// SPDX-License-Identifier: BSL-1.0
|
||||
/*!
|
||||
* @file
|
||||
* @brief Setter-upper for kinematic model
|
||||
* @author Moses Turner <moses@collabora.com>
|
||||
* @ingroup tracking
|
||||
*/
|
||||
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "ccdik_defines.hpp"
|
||||
#include "ccdik_tiny_math.hpp"
|
||||
|
||||
|
||||
namespace xrt::tracking::hand::mercury::ccdik {
|
||||
|
||||
void
|
||||
bone_update_quat_and_matrix(struct bone_t *bone)
|
||||
{
|
||||
wct_to_quat(bone->rot_to_next_joint_wct, (xrt_quat *)&bone->rot_to_next_joint_quat);
|
||||
|
||||
// To make sure that the bottom-right cell is 1.0f. You can do this somewhere else (just once) for speed
|
||||
bone->bone_relation.setIdentity();
|
||||
bone->bone_relation.translation() = bone->trans_from_last_joint;
|
||||
bone->bone_relation.linear() = Eigen::Matrix3f(bone->rot_to_next_joint_quat);
|
||||
}
|
||||
|
||||
void
|
||||
eval_chain(std::vector<const Eigen::Affine3f *> &chain, Eigen::Affine3f &out)
|
||||
{
|
||||
out.setIdentity();
|
||||
for (const Eigen::Affine3f *new_matrix : chain) {
|
||||
// I have NO IDEA if it's correct to left-multiply or right-multiply.
|
||||
// I need to go to school for linear algebra for a lot longer...
|
||||
out = out * (*new_matrix);
|
||||
}
|
||||
}
|
||||
|
||||
void
|
||||
_statics_init_world_parents(KinematicHandCCDIK *hand)
|
||||
{
|
||||
for (int finger = 0; finger < 5; finger++) {
|
||||
finger_t *of = &hand->fingers[finger];
|
||||
of->bones[0].parent.world_pose = &hand->wrist_relation;
|
||||
|
||||
// Does this make any sense? Be careful here...
|
||||
of->bones[0].parent.bone_relation = &hand->wrist_relation;
|
||||
|
||||
for (int bone = 1; bone < 5; bone++) {
|
||||
of->bones[bone].parent.world_pose = &of->bones[bone - 1].world_pose;
|
||||
of->bones[bone].parent.bone_relation = &of->bones[bone - 1].bone_relation;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void
|
||||
_statics_init_world_poses(KinematicHandCCDIK *hand)
|
||||
{
|
||||
XRT_TRACE_MARKER();
|
||||
for (int finger = 0; finger < 5; finger++) {
|
||||
finger_t *of = &hand->fingers[finger];
|
||||
|
||||
for (int bone = 0; bone < 5; bone++) {
|
||||
of->bones[bone].world_pose =
|
||||
(*of->bones[bone].parent.world_pose) * of->bones[bone].bone_relation;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void
|
||||
_statics_init_loc_ptrs(KinematicHandCCDIK *hand)
|
||||
{
|
||||
hand->fingers[0].bones[1].keypoint_idx_21 = Joint21::THMB_MCP;
|
||||
hand->fingers[0].bones[2].keypoint_idx_21 = Joint21::THMB_PXM;
|
||||
hand->fingers[0].bones[3].keypoint_idx_21 = Joint21::THMB_DST;
|
||||
hand->fingers[0].bones[4].keypoint_idx_21 = Joint21::THMB_TIP;
|
||||
|
||||
hand->fingers[1].bones[1].keypoint_idx_21 = Joint21::INDX_PXM;
|
||||
hand->fingers[1].bones[2].keypoint_idx_21 = Joint21::INDX_INT;
|
||||
hand->fingers[1].bones[3].keypoint_idx_21 = Joint21::INDX_DST;
|
||||
hand->fingers[1].bones[4].keypoint_idx_21 = Joint21::INDX_TIP;
|
||||
|
||||
hand->fingers[2].bones[1].keypoint_idx_21 = Joint21::MIDL_PXM;
|
||||
hand->fingers[2].bones[2].keypoint_idx_21 = Joint21::MIDL_INT;
|
||||
hand->fingers[2].bones[3].keypoint_idx_21 = Joint21::MIDL_DST;
|
||||
hand->fingers[2].bones[4].keypoint_idx_21 = Joint21::MIDL_TIP;
|
||||
|
||||
hand->fingers[3].bones[1].keypoint_idx_21 = Joint21::RING_PXM;
|
||||
hand->fingers[3].bones[2].keypoint_idx_21 = Joint21::RING_INT;
|
||||
hand->fingers[3].bones[3].keypoint_idx_21 = Joint21::RING_DST;
|
||||
hand->fingers[3].bones[4].keypoint_idx_21 = Joint21::RING_TIP;
|
||||
|
||||
hand->fingers[4].bones[1].keypoint_idx_21 = Joint21::LITL_PXM;
|
||||
hand->fingers[4].bones[2].keypoint_idx_21 = Joint21::LITL_INT;
|
||||
hand->fingers[4].bones[3].keypoint_idx_21 = Joint21::LITL_DST;
|
||||
hand->fingers[4].bones[4].keypoint_idx_21 = Joint21::LITL_TIP;
|
||||
}
|
||||
|
||||
void
|
||||
_statics_joint_limits(KinematicHandCCDIK *hand)
|
||||
{
|
||||
{
|
||||
finger_t *t = &hand->fingers[0];
|
||||
t->bones[1].joint_limit_max.waggle = rad(70);
|
||||
t->bones[1].joint_limit_min.waggle = rad(-70);
|
||||
|
||||
t->bones[1].joint_limit_max.curl = rad(70);
|
||||
t->bones[1].joint_limit_min.curl = rad(-70);
|
||||
|
||||
t->bones[1].joint_limit_max.twist = rad(40);
|
||||
t->bones[1].joint_limit_min.twist = rad(-40);
|
||||
|
||||
//
|
||||
|
||||
t->bones[2].joint_limit_max.waggle = rad(0);
|
||||
t->bones[2].joint_limit_min.waggle = rad(0);
|
||||
|
||||
t->bones[2].joint_limit_max.curl = rad(50);
|
||||
t->bones[2].joint_limit_min.curl = rad(-100);
|
||||
|
||||
t->bones[2].joint_limit_max.twist = rad(0);
|
||||
t->bones[2].joint_limit_min.twist = rad(0);
|
||||
|
||||
//
|
||||
|
||||
t->bones[3].joint_limit_max.waggle = rad(0);
|
||||
t->bones[3].joint_limit_min.waggle = rad(0);
|
||||
|
||||
t->bones[3].joint_limit_max.curl = rad(50);
|
||||
t->bones[3].joint_limit_min.curl = rad(-100);
|
||||
|
||||
t->bones[3].joint_limit_max.twist = rad(0);
|
||||
t->bones[3].joint_limit_min.twist = rad(0);
|
||||
}
|
||||
}
|
||||
|
||||
// Exported:
|
||||
void
|
||||
init_hardcoded_statics(KinematicHandCCDIK *hand, float size)
|
||||
{
|
||||
hand->size = size;
|
||||
hand->wrist_relation.setIdentity();
|
||||
hand->wrist_relation.linear() *= hand->size;
|
||||
|
||||
{
|
||||
|
||||
finger_t *t = &hand->fingers[0];
|
||||
|
||||
// Hidden extra bone that makes our code easier to write. Note the weird extra rotation.
|
||||
t->bones[0].rot_to_next_joint_wct = {-rad(45), rad(-10), -rad(70)};
|
||||
t->bones[0].trans_from_last_joint = {0.33097, 0, -0.25968};
|
||||
|
||||
t->bones[1].rot_to_next_joint_wct = {0, rad(-5), 0};
|
||||
|
||||
t->bones[2].rot_to_next_joint_wct = {0, rad(-25), 0};
|
||||
t->bones[2].trans_from_last_joint.z() = -0.389626;
|
||||
|
||||
t->bones[3].rot_to_next_joint_wct = {0, rad(-25), 0};
|
||||
t->bones[3].trans_from_last_joint.z() = -0.311176;
|
||||
|
||||
t->bones[4].trans_from_last_joint.z() = -0.232195;
|
||||
}
|
||||
|
||||
|
||||
float wagg = -0.19;
|
||||
|
||||
float finger_joints[4][3] = {
|
||||
{
|
||||
-0.365719,
|
||||
-0.231581,
|
||||
-0.201790,
|
||||
},
|
||||
{
|
||||
-0.404486,
|
||||
-0.247749,
|
||||
-0.210121,
|
||||
},
|
||||
{
|
||||
-0.365639,
|
||||
-0.225666,
|
||||
-0.187089,
|
||||
},
|
||||
{
|
||||
-0.278197,
|
||||
-0.176178,
|
||||
-0.157566,
|
||||
},
|
||||
};
|
||||
|
||||
for (int finger = HF_INDEX; finger <= HF_LITTLE; finger++) {
|
||||
finger_t *of = &hand->fingers[finger];
|
||||
of->bones[0].rot_to_next_joint_wct.waggle = wagg;
|
||||
wagg += 0.19f;
|
||||
|
||||
of->bones[FB_PROXIMAL].rot_to_next_joint_wct.curl = rad(-5);
|
||||
of->bones[FB_INTERMEDIATE].rot_to_next_joint_wct.curl = rad(-5);
|
||||
of->bones[FB_DISTAL].rot_to_next_joint_wct.curl = rad(-5);
|
||||
|
||||
|
||||
for (int i = 0; i < 3; i++) {
|
||||
int bone = i + 2;
|
||||
of->bones[bone].trans_from_last_joint.x() = 0;
|
||||
of->bones[bone].trans_from_last_joint.y() = 0;
|
||||
of->bones[bone].trans_from_last_joint.z() = finger_joints[finger - 1][i];
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
hand->fingers[1].bones[1].trans_from_last_joint.z() = -0.66;
|
||||
hand->fingers[2].bones[1].trans_from_last_joint.z() = -0.645;
|
||||
hand->fingers[3].bones[1].trans_from_last_joint.z() = -0.58;
|
||||
hand->fingers[4].bones[1].trans_from_last_joint.z() = -0.52;
|
||||
|
||||
hand->fingers[HF_INDEX].bones[0].trans_from_last_joint = {0.16926, 0, -0.34437};
|
||||
hand->fingers[HF_MIDDLE].bones[0].trans_from_last_joint = {0.034639, 0, -0.35573};
|
||||
hand->fingers[HF_RING].bones[0].trans_from_last_joint = {-0.063625, 0, -0.34164};
|
||||
hand->fingers[HF_LITTLE].bones[0].trans_from_last_joint = {-0.1509, 0, -0.30373};
|
||||
|
||||
for (int finger_idx = 0; finger_idx < 5; finger_idx++) {
|
||||
for (int bone_idx = 0; bone_idx < 5; bone_idx++) {
|
||||
bone_update_quat_and_matrix(&hand->fingers[finger_idx].bones[bone_idx]);
|
||||
}
|
||||
}
|
||||
|
||||
_statics_init_world_parents(hand);
|
||||
_statics_init_world_poses(hand);
|
||||
_statics_init_loc_ptrs(hand);
|
||||
_statics_joint_limits(hand);
|
||||
}
|
||||
} // namespace xrt::tracking::hand::mercury::ccdik
|
|
@ -1,31 +0,0 @@
|
|||
// Copyright 2022, Collabora, Ltd.
|
||||
// SPDX-License-Identifier: BSL-1.0
|
||||
/*!
|
||||
* @file
|
||||
* @brief Interface for Cyclic Coordinate Descent IK kinematic optimizer
|
||||
* @author Moses Turner <moses@collabora.com>
|
||||
* @ingroup tracking
|
||||
*/
|
||||
#pragma once
|
||||
// #include "math/m_api.h"
|
||||
|
||||
#include "xrt/xrt_defines.h"
|
||||
#include "../kine_common.hpp"
|
||||
|
||||
namespace xrt::tracking::hand::mercury::ccdik {
|
||||
|
||||
struct KinematicHandCCDIK;
|
||||
|
||||
void
|
||||
alloc_kinematic_hand(xrt_pose left_in_right, bool is_right, KinematicHandCCDIK **out_kinematic_hand);
|
||||
|
||||
void
|
||||
optimize_new_frame(KinematicHandCCDIK *hand, one_frame_input &observation, struct xrt_hand_joint_set &out_set);
|
||||
|
||||
void
|
||||
init_hardcoded_statics(KinematicHandCCDIK *hand, float size = 0.095864);
|
||||
|
||||
void
|
||||
free_kinematic_hand(KinematicHandCCDIK **kinematic_hand);
|
||||
|
||||
} // namespace xrt::tracking::hand::mercury::ccdik
|
|
@ -1,509 +0,0 @@
|
|||
// Copyright 2022, Collabora, Ltd.
|
||||
// SPDX-License-Identifier: BSL-1.0
|
||||
/*!
|
||||
* @file
|
||||
* @brief Main code for kinematic model
|
||||
* @author Moses Turner <moses@collabora.com>
|
||||
* @ingroup tracking
|
||||
*/
|
||||
|
||||
#include "ccdik_defines.hpp"
|
||||
#include "ccdik_tiny_math.hpp"
|
||||
#include "ccdik_hand_init.hpp"
|
||||
#include "lineline.hpp"
|
||||
#include "math/m_api.h"
|
||||
#include "util/u_logging.h"
|
||||
|
||||
#include <Eigen/Core>
|
||||
#include <Eigen/LU>
|
||||
#include <Eigen/SVD>
|
||||
#include <Eigen/src/Geometry/Umeyama.h>
|
||||
|
||||
|
||||
|
||||
namespace xrt::tracking::hand::mercury::ccdik {
|
||||
|
||||
static void
|
||||
_two_set_ele(Eigen::Matrix<float, 3, kNumNNJoints> &thing, Eigen::Affine3f jt, int idx)
|
||||
{
|
||||
//! @todo Optimize
|
||||
thing.col(idx) = jt.translation();
|
||||
}
|
||||
|
||||
static void
|
||||
two(struct KinematicHandCCDIK *hand)
|
||||
{
|
||||
XRT_TRACE_MARKER();
|
||||
|
||||
|
||||
int i = 0;
|
||||
|
||||
_two_set_ele(hand->kinematic, hand->wrist_relation, i);
|
||||
|
||||
for (int finger_idx = 0; finger_idx < 5; finger_idx++) {
|
||||
for (int bone_idx = 1; bone_idx < 5; bone_idx++) {
|
||||
|
||||
i++;
|
||||
|
||||
_two_set_ele(hand->kinematic, hand->fingers[finger_idx].bones[bone_idx].world_pose, i);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
Eigen::Affine3f aff3d;
|
||||
|
||||
aff3d = Eigen::umeyama(hand->kinematic, hand->t_jts_as_mat, false);
|
||||
|
||||
hand->wrist_relation = aff3d * hand->wrist_relation;
|
||||
|
||||
_statics_init_world_poses(hand);
|
||||
}
|
||||
|
||||
#if 0
|
||||
#define DOT_EPSILON 0.000001f
|
||||
|
||||
Eigen::Quaternionf
|
||||
fast_simple_rotation(const Eigen::Vector3f &from_un, const Eigen::Vector3f &to_un)
|
||||
{
|
||||
// Slooowwwwwww....
|
||||
Eigen::Vector3f from = from_un.normalized();
|
||||
Eigen::Vector3f to = to_un.normalized();
|
||||
|
||||
Eigen::Vector3f axis = from.cross(to);
|
||||
float dot = from.dot(to);
|
||||
|
||||
if (dot < (-1.0f + DOT_EPSILON)) {
|
||||
return Eigen::Quaternionf(0, 1, 0, 0);
|
||||
};
|
||||
|
||||
Eigen::Quaternionf result(axis.x() * 0.5f, axis.y() * 0.5f, axis.z() * 0.5f, (dot + 1.0f) * 0.5f);
|
||||
result.normalize();
|
||||
return result;
|
||||
}
|
||||
|
||||
Eigen::Matrix3f
|
||||
moses_fast_simple_rotation(const Eigen::Vector3f &from_un, const Eigen::Vector3f &to_un)
|
||||
{
|
||||
// Slooowwwwwww....
|
||||
Eigen::Vector3f from = from_un.normalized();
|
||||
Eigen::Vector3f to = to_un.normalized();
|
||||
|
||||
Eigen::Vector3f axis = from.cross(to).normalized();
|
||||
float angle = acos(from.dot(to));
|
||||
|
||||
// U_LOG_E("HERE %f", angle);
|
||||
// std::cout << to << std::endl;
|
||||
// std::cout << from << std::endl;
|
||||
// std::cout << axis << std::endl;
|
||||
// std::cout << angle << std::endl;
|
||||
|
||||
Eigen::AngleAxisf ax;
|
||||
|
||||
ax.axis() = axis;
|
||||
ax.angle() = angle;
|
||||
|
||||
return ax.matrix();
|
||||
}
|
||||
#endif
|
||||
|
||||
|
||||
static void
|
||||
do_it_for_bone(struct KinematicHandCCDIK *hand, int finger_idx, int bone_idx, bool clamp_to_x_axis_rotation)
|
||||
{
|
||||
finger_t *of = &hand->fingers[finger_idx];
|
||||
bone_t *bone = &hand->fingers[finger_idx].bones[bone_idx];
|
||||
int num_children = 0;
|
||||
|
||||
Eigen::Vector3f kine = Eigen::Vector3f::Zero();
|
||||
Eigen::Vector3f target = Eigen::Vector3f::Zero();
|
||||
|
||||
|
||||
for (int idx = bone_idx + 1; idx < 5; idx++) {
|
||||
num_children++;
|
||||
target += map_vec3(hand->t_jts[of->bones[idx].keypoint_idx_21]);
|
||||
kine += of->bones[idx].world_pose.translation();
|
||||
}
|
||||
|
||||
kine *= 1.0f / (float)num_children;
|
||||
target *= 1.0f / (float)num_children;
|
||||
|
||||
kine = bone->world_pose.inverse() * kine;
|
||||
target = bone->world_pose.inverse() * target;
|
||||
|
||||
kine.normalize();
|
||||
target.normalize();
|
||||
|
||||
Eigen::Matrix3f rot = Eigen::Quaternionf().setFromTwoVectors(kine, target).matrix();
|
||||
|
||||
bone->bone_relation.linear() = bone->bone_relation.linear() * rot;
|
||||
}
|
||||
|
||||
static void
|
||||
clamp_to_x_axis(struct KinematicHandCCDIK *hand,
|
||||
int finger_idx,
|
||||
int bone_idx,
|
||||
bool clamp_angle = false,
|
||||
float min_angle = 0,
|
||||
float max_angle = 0)
|
||||
{
|
||||
bone_t *bone = &hand->fingers[finger_idx].bones[bone_idx];
|
||||
// U_LOG_E("before_anything");
|
||||
|
||||
// std::cout << bone->bone_relation.linear().matrix() << std::endl;
|
||||
|
||||
|
||||
|
||||
Eigen::Vector3f new_x = bone->bone_relation.linear() * Eigen::Vector3f::UnitX();
|
||||
Eigen::Matrix3f correction_rot =
|
||||
Eigen::Quaternionf().setFromTwoVectors(new_x.normalized(), Eigen::Vector3f::UnitX()).matrix();
|
||||
// U_LOG_E("correction");
|
||||
|
||||
// std::cout << correction_rot << std::endl;
|
||||
|
||||
// U_LOG_E("correction times new_x");
|
||||
// std::cout << correction_rot * new_x << "\n";
|
||||
|
||||
// U_LOG_E("before, where does relation point x");
|
||||
// std::cout << bone->bone_relation.rotation() * Eigen::Vector3f::UnitX() << "\n";
|
||||
|
||||
|
||||
// Weird that we're left-multiplying here, I don't know why. But it works.
|
||||
bone->bone_relation.linear() = correction_rot * bone->bone_relation.linear();
|
||||
|
||||
// U_LOG_E("after_correction");
|
||||
|
||||
|
||||
// std::cout << bone->bone_relation.linear() << std::endl;
|
||||
|
||||
|
||||
// U_LOG_E("after, where does relation point x");
|
||||
// std::cout << bone->bone_relation.linear() * Eigen::Vector3f::UnitX() << "\n";
|
||||
|
||||
if (clamp_angle) {
|
||||
//! @todo Optimize: get rid of 1 and 2, we only need 0.
|
||||
|
||||
// signed angle: asin(Cross product of -z and rot*-z X axis.
|
||||
// U_LOG_E("before X clamp");
|
||||
// std::cout << bone->bone_relation.linear() << "\n";
|
||||
|
||||
// auto euler = bone->bone_relation.linear().eulerAngles(0, 1, 2);
|
||||
|
||||
Eigen::Vector3f cross =
|
||||
(-Eigen::Vector3f::UnitZ()).cross(bone->bone_relation.linear() * (-Eigen::Vector3f::UnitZ()));
|
||||
|
||||
// std::cout << bone->bone_relation.linear() << "\n";
|
||||
|
||||
|
||||
// U_LOG_E("eluer before clamp: %f %f %f %f %f", min_angle, max_angle, euler(0), euler(1), euler(2));
|
||||
// U_LOG_E("eluer before clamp: %f %f %f %f %f %f", euler(0), euler(1), euler(2), cross(0), cross(1) ,
|
||||
// cross(2));
|
||||
|
||||
|
||||
|
||||
//! @todo Optimize: Move the asin into constexpr land
|
||||
// No, the sine of the joint limit
|
||||
float rotation_value = asin(cross(0));
|
||||
|
||||
//!@bug No. If the rotation value is outside of the allowed values, clamp it to the rotation it's
|
||||
//!*closest to*.
|
||||
// That's different than using clamp, rotation formalisms are complicated.
|
||||
clamp(&rotation_value, min_angle, max_angle);
|
||||
|
||||
// U_LOG_E("eluer after clamp: %f", rotation_value);
|
||||
|
||||
|
||||
Eigen::Matrix3f n;
|
||||
n = Eigen::AngleAxisf(rotation_value, Eigen::Vector3f::UnitX());
|
||||
bone->bone_relation.linear() = n;
|
||||
// U_LOG_E("after X clamp");
|
||||
|
||||
// std::cout << n << "\n";
|
||||
}
|
||||
}
|
||||
|
||||
// Is this not just swing-twist about the Z axis? Dunnoooo... Find out later.
|
||||
static void
|
||||
clamp_proximals(struct KinematicHandCCDIK *hand,
|
||||
int finger_idx,
|
||||
int bone_idx,
|
||||
float max_swing_angle = 0,
|
||||
float tanangle_left = tan(rad(-20)),
|
||||
float tanangle_right = tan(rad(20)),
|
||||
float tanangle_curled = tan(rad(-89)), // Uh oh...
|
||||
float tanangle_uncurled = tan(rad(30)))
|
||||
{
|
||||
bone_t *bone = &hand->fingers[finger_idx].bones[bone_idx];
|
||||
|
||||
|
||||
Eigen::Matrix3f rot = bone->bone_relation.linear();
|
||||
|
||||
// U_LOG_E("rot");
|
||||
// std::cout << rot << "\n";
|
||||
|
||||
Eigen::Vector3f our_z = rot * (-Eigen::Vector3f::UnitZ());
|
||||
|
||||
Eigen::Matrix3f simple = Eigen::Quaternionf().setFromTwoVectors(-Eigen::Vector3f::UnitZ(), our_z).matrix();
|
||||
// U_LOG_E("simple");
|
||||
// std::cout << simple << "\n";
|
||||
|
||||
Eigen::Matrix3f twist = rot * simple.inverse();
|
||||
// U_LOG_E("twist");
|
||||
|
||||
// std::cout << twist << "\n";
|
||||
|
||||
// U_LOG_E("twist_axis");
|
||||
|
||||
Eigen::AngleAxisf twist_aax = Eigen::AngleAxisf(twist);
|
||||
|
||||
// std::cout << twist_aax.axis() << "\n";
|
||||
|
||||
// U_LOG_E("twist_angle");
|
||||
|
||||
// std::cout << twist_aax.angle() << "\n";
|
||||
|
||||
|
||||
|
||||
// U_LOG_E("all together now");
|
||||
|
||||
// std::cout << twist * simple << "\n";
|
||||
|
||||
if (fabs(twist_aax.angle()) > max_swing_angle) {
|
||||
// max_swing_angle times +1 or -1, depending.
|
||||
twist_aax.angle() = max_swing_angle * (twist_aax.angle() / fabs(twist_aax.angle()));
|
||||
// U_LOG_E("clamping twist %f", twist_aax.angle());
|
||||
|
||||
|
||||
// std::cout << twist << "\n";
|
||||
// std::cout << twist_aax.toRotationMatrix() << "\n";
|
||||
}
|
||||
|
||||
|
||||
if (our_z.z() > 0) {
|
||||
//!@bug We need smarter joint limiting, limiting via tanangles is not acceptable as joints can rotate
|
||||
//! outside of the 180 degree hemisphere.
|
||||
our_z.z() = -0.000001f;
|
||||
}
|
||||
our_z *= -1.0f / our_z.z();
|
||||
|
||||
|
||||
clamp(&our_z.x(), tanangle_left, tanangle_right);
|
||||
clamp(&our_z.y(), tanangle_curled, tanangle_uncurled);
|
||||
|
||||
simple = Eigen::Quaternionf().setFromTwoVectors(-Eigen::Vector3f::UnitZ(), our_z.normalized()).matrix();
|
||||
|
||||
bone->bone_relation.linear() = twist_aax * simple;
|
||||
}
|
||||
|
||||
|
||||
|
||||
static void
|
||||
do_it_for_finger(struct KinematicHandCCDIK *hand, int finger_idx)
|
||||
{
|
||||
do_it_for_bone(hand, finger_idx, 0, false);
|
||||
clamp_proximals(hand, finger_idx, 0, rad(4.0), tan(rad(-30)), tan(rad(30)), tan(rad(-10)), tan(rad(10)));
|
||||
_statics_init_world_poses(hand);
|
||||
|
||||
do_it_for_bone(hand, finger_idx, 1, true);
|
||||
clamp_proximals(hand, finger_idx, 1, rad(4.0));
|
||||
_statics_init_world_poses(hand);
|
||||
|
||||
|
||||
do_it_for_bone(hand, finger_idx, 2, true);
|
||||
clamp_to_x_axis(hand, finger_idx, 2, true, rad(-90), rad(10));
|
||||
_statics_init_world_poses(hand);
|
||||
|
||||
do_it_for_bone(hand, finger_idx, 3, true);
|
||||
clamp_to_x_axis(hand, finger_idx, 3, true, rad(-90), rad(10));
|
||||
_statics_init_world_poses(hand);
|
||||
}
|
||||
|
||||
static void
|
||||
optimize(KinematicHandCCDIK *hand)
|
||||
{
|
||||
for (int i = 0; i < 15; i++) {
|
||||
two(hand);
|
||||
do_it_for_bone(hand, 0, 1, false);
|
||||
clamp_proximals(hand, 0, 1, rad(70), tan(rad(-40)), tan(rad(40)), tan(rad(-40)), tan(rad(40)));
|
||||
_statics_init_world_poses(hand);
|
||||
|
||||
do_it_for_bone(hand, 0, 2, true);
|
||||
clamp_to_x_axis(hand, 0, 2, true, rad(-90), rad(40));
|
||||
_statics_init_world_poses(hand);
|
||||
|
||||
do_it_for_bone(hand, 0, 3, true);
|
||||
clamp_to_x_axis(hand, 0, 3, true, rad(-90), rad(40));
|
||||
_statics_init_world_poses(hand);
|
||||
|
||||
two(hand);
|
||||
|
||||
do_it_for_finger(hand, 1);
|
||||
do_it_for_finger(hand, 2);
|
||||
do_it_for_finger(hand, 3);
|
||||
do_it_for_finger(hand, 4);
|
||||
}
|
||||
two(hand);
|
||||
}
|
||||
|
||||
|
||||
static void
|
||||
make_joint_at_matrix_left_hand(int idx, Eigen::Affine3f &pose, struct xrt_hand_joint_set &hand)
|
||||
{
|
||||
hand.values.hand_joint_set_default[idx].relation.relation_flags = (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);
|
||||
Eigen::Vector3f v = pose.translation();
|
||||
hand.values.hand_joint_set_default[idx].relation.pose.position.x = v.x();
|
||||
hand.values.hand_joint_set_default[idx].relation.pose.position.y = v.y();
|
||||
hand.values.hand_joint_set_default[idx].relation.pose.position.z = v.z();
|
||||
|
||||
Eigen::Quaternionf q;
|
||||
q = pose.rotation();
|
||||
|
||||
hand.values.hand_joint_set_default[idx].relation.pose.orientation.x = q.x();
|
||||
hand.values.hand_joint_set_default[idx].relation.pose.orientation.y = q.y();
|
||||
hand.values.hand_joint_set_default[idx].relation.pose.orientation.z = q.z();
|
||||
hand.values.hand_joint_set_default[idx].relation.pose.orientation.w = q.w();
|
||||
}
|
||||
|
||||
static void
|
||||
make_joint_at_matrix_right_hand(int idx, Eigen::Affine3f &pose, struct xrt_hand_joint_set &hand)
|
||||
{
|
||||
hand.values.hand_joint_set_default[idx].relation.relation_flags = (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);
|
||||
Eigen::Vector3f v = pose.translation();
|
||||
hand.values.hand_joint_set_default[idx].relation.pose.position.x = -v.x();
|
||||
hand.values.hand_joint_set_default[idx].relation.pose.position.y = v.y();
|
||||
hand.values.hand_joint_set_default[idx].relation.pose.position.z = v.z();
|
||||
|
||||
Eigen::Matrix3f rotation = pose.rotation();
|
||||
|
||||
Eigen::Matrix3f mirror_on_x = Eigen::Matrix3f::Identity();
|
||||
mirror_on_x(0, 0) = -1;
|
||||
|
||||
rotation = mirror_on_x * rotation;
|
||||
|
||||
rotation(0, 0) *= -1;
|
||||
rotation(1, 0) *= -1;
|
||||
rotation(2, 0) *= -1;
|
||||
|
||||
Eigen::Quaternionf q;
|
||||
|
||||
q = rotation;
|
||||
|
||||
hand.values.hand_joint_set_default[idx].relation.pose.orientation.x = q.x();
|
||||
hand.values.hand_joint_set_default[idx].relation.pose.orientation.y = q.y();
|
||||
hand.values.hand_joint_set_default[idx].relation.pose.orientation.z = q.z();
|
||||
hand.values.hand_joint_set_default[idx].relation.pose.orientation.w = q.w();
|
||||
}
|
||||
|
||||
static void
|
||||
make_joint_at_matrix(int idx, Eigen::Affine3f &pose, struct xrt_hand_joint_set &hand, bool is_right)
|
||||
{
|
||||
if (!is_right) {
|
||||
make_joint_at_matrix_left_hand(idx, pose, hand);
|
||||
} else {
|
||||
make_joint_at_matrix_right_hand(idx, pose, hand);
|
||||
}
|
||||
}
|
||||
|
||||
// Exported:
|
||||
void
|
||||
optimize_new_frame(KinematicHandCCDIK *hand, one_frame_input &observation, struct xrt_hand_joint_set &out_set)
|
||||
{
|
||||
|
||||
// intake poses!
|
||||
for (int i = 0; i < 21; i++) {
|
||||
|
||||
xrt_vec3 p1 = {0, 0, 0};
|
||||
xrt_vec3 p2 = observation.views[0].rays[i];
|
||||
|
||||
xrt_vec3 p3 = hand->right_in_left.position;
|
||||
|
||||
xrt_vec3 p4;
|
||||
math_quat_rotate_vec3(&hand->right_in_left.orientation, &observation.views[1].rays[i], &p4);
|
||||
p4 += hand->right_in_left.position;
|
||||
|
||||
xrt_vec3 pa;
|
||||
xrt_vec3 pb;
|
||||
float mua;
|
||||
float mub;
|
||||
|
||||
LineLineIntersect(p1, p2, p3, p4, &pa, &pb, &mua, &mub);
|
||||
|
||||
xrt_vec3 p;
|
||||
p = pa + pb;
|
||||
math_vec3_scalar_mul(0.5, &p);
|
||||
|
||||
|
||||
if (!hand->is_right) {
|
||||
|
||||
hand->t_jts[i] = p;
|
||||
} else {
|
||||
hand->t_jts[i].x = -p.x;
|
||||
hand->t_jts[i].y = p.y;
|
||||
hand->t_jts[i].z = p.z;
|
||||
}
|
||||
|
||||
hand->t_jts_as_mat(0, i) = hand->t_jts[i].x;
|
||||
hand->t_jts_as_mat(1, i) = hand->t_jts[i].y;
|
||||
hand->t_jts_as_mat(2, i) = hand->t_jts[i].z;
|
||||
}
|
||||
|
||||
// do the math!
|
||||
optimize(hand);
|
||||
|
||||
// Convert it to xrt_hand_joint_set!
|
||||
|
||||
make_joint_at_matrix(XRT_HAND_JOINT_WRIST, hand->wrist_relation, out_set, hand->is_right);
|
||||
|
||||
Eigen::Affine3f palm_relation;
|
||||
|
||||
palm_relation.linear() = hand->fingers[2].bones[0].world_pose.linear();
|
||||
|
||||
palm_relation.translation() = Eigen::Vector3f::Zero();
|
||||
palm_relation.translation() += hand->fingers[2].bones[0].world_pose.translation() / 2;
|
||||
palm_relation.translation() += hand->fingers[2].bones[1].world_pose.translation() / 2;
|
||||
|
||||
make_joint_at_matrix(XRT_HAND_JOINT_PALM, palm_relation, out_set, hand->is_right);
|
||||
|
||||
int start = XRT_HAND_JOINT_THUMB_METACARPAL;
|
||||
|
||||
for (int finger_idx = 0; finger_idx < 5; finger_idx++) {
|
||||
|
||||
for (int bone_idx = 0; bone_idx < 5; bone_idx++) {
|
||||
CONTINUE_IF_HIDDEN_THUMB;
|
||||
|
||||
if (!(finger_idx == 0 && bone_idx == 0)) {
|
||||
make_joint_at_matrix(start++, hand->fingers[finger_idx].bones[bone_idx].world_pose,
|
||||
out_set, hand->is_right);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
out_set.is_active = true;
|
||||
}
|
||||
|
||||
void
|
||||
alloc_kinematic_hand(xrt_pose left_in_right, bool is_right, KinematicHandCCDIK **out_kinematic_hand)
|
||||
{
|
||||
KinematicHandCCDIK *h = new KinematicHandCCDIK();
|
||||
h->is_right = is_right;
|
||||
|
||||
math_pose_invert(&left_in_right, &h->right_in_left);
|
||||
|
||||
// U_LOG_E("%f %f %f", h->right_in_left.position.x, h->right_in_left.position.y, h->right_in_left.position.z);
|
||||
|
||||
// Doesn't matter, should get overwritten later.
|
||||
init_hardcoded_statics(h, 0.09f);
|
||||
|
||||
*out_kinematic_hand = h;
|
||||
}
|
||||
|
||||
void
|
||||
free_kinematic_hand(KinematicHandCCDIK **kinematic_hand)
|
||||
{
|
||||
delete *kinematic_hand;
|
||||
}
|
||||
|
||||
} // namespace xrt::tracking::hand::mercury::ccdik
|
|
@ -1,57 +0,0 @@
|
|||
// Copyright 2022, Collabora, Ltd.
|
||||
// SPDX-License-Identifier: BSL-1.0
|
||||
/*!
|
||||
* @file
|
||||
* @brief Math for kinematic model
|
||||
* @author Moses Turner <moses@collabora.com>
|
||||
* @ingroup tracking
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
#include "ccdik_defines.hpp"
|
||||
|
||||
namespace xrt::tracking::hand::mercury::ccdik {
|
||||
// Waggle-curl-twist.
|
||||
static inline void
|
||||
wct_to_quat(wct_t wct, struct xrt_quat *out)
|
||||
{
|
||||
XRT_TRACE_MARKER();
|
||||
xrt_vec3 waggle_axis = {0, 1, 0};
|
||||
xrt_quat just_waggle;
|
||||
math_quat_from_angle_vector(wct.waggle, &waggle_axis, &just_waggle);
|
||||
|
||||
xrt_vec3 curl_axis = {1, 0, 0};
|
||||
xrt_quat just_curl;
|
||||
math_quat_from_angle_vector(wct.curl, &curl_axis, &just_curl);
|
||||
|
||||
xrt_vec3 twist_axis = {0, 0, 1};
|
||||
xrt_quat just_twist;
|
||||
math_quat_from_angle_vector(wct.twist, &twist_axis, &just_twist);
|
||||
|
||||
//! @todo Optimize This should be a matrix multiplication...
|
||||
// Are you sure about that, previous moses? Pretty sure that quat products are faster than 3x3 matrix
|
||||
// products...
|
||||
*out = just_waggle;
|
||||
math_quat_rotate(out, &just_curl, out);
|
||||
math_quat_rotate(out, &just_twist, out);
|
||||
}
|
||||
|
||||
// Inlines.
|
||||
static inline float
|
||||
rad(double degrees)
|
||||
{
|
||||
return degrees * (M_PI / 180.0);
|
||||
}
|
||||
|
||||
static inline void
|
||||
clamp(float *in, float min, float max)
|
||||
{
|
||||
*in = fminf(max, fmaxf(min, *in));
|
||||
}
|
||||
|
||||
static inline void
|
||||
clamp_to_r(float *in, float c, float r)
|
||||
{
|
||||
clamp(in, c - r, c + r);
|
||||
}
|
||||
} // namespace xrt::tracking::hand::mercury::ccdik
|
|
@ -1,89 +0,0 @@
|
|||
// Copyright 1998, Paul Bourke.
|
||||
// Copyright 2022, Collabora, Ltd.
|
||||
// SPDX-License-Identifier: BSL-1.0
|
||||
/*!
|
||||
* @file
|
||||
* @brief Find the closest approach between two lines
|
||||
* @author Paul Bourke <paul.bourke@gmail.com>
|
||||
* @author Moses Turner <moses@collabora.com>
|
||||
* @ingroup tracking
|
||||
*/
|
||||
#include "float.h"
|
||||
#include <limits.h>
|
||||
#include <math.h>
|
||||
#include "util/u_logging.h"
|
||||
#include "xrt/xrt_defines.h"
|
||||
#include "stdbool.h"
|
||||
|
||||
// Taken and relicensed with written permission from http://paulbourke.net/geometry/pointlineplane/lineline.c +
|
||||
// http://paulbourke.net/geometry/pointlineplane/
|
||||
|
||||
/*
|
||||
Calculate the line segment PaPb that is the shortest route between
|
||||
two lines P1P2 and P3P4. Calculate also the values of mua and mub where
|
||||
Pa = P1 + mua (P2 - P1)
|
||||
Pb = P3 + mub (P4 - P3)
|
||||
Return false if no solution exists.
|
||||
*/
|
||||
bool
|
||||
LineLineIntersect(struct xrt_vec3 p1,
|
||||
struct xrt_vec3 p2,
|
||||
struct xrt_vec3 p3,
|
||||
struct xrt_vec3 p4,
|
||||
struct xrt_vec3 *pa,
|
||||
struct xrt_vec3 *pb,
|
||||
float *mua,
|
||||
float *mub)
|
||||
{
|
||||
struct xrt_vec3 p13, p43, p21; // NOLINT
|
||||
float d1343, d4321, d1321, d4343, d2121; // NOLINT
|
||||
float number, denom; // NOLINT
|
||||
|
||||
p13.x = p1.x - p3.x;
|
||||
p13.y = p1.y - p3.y;
|
||||
p13.z = p1.z - p3.z;
|
||||
|
||||
p43.x = p4.x - p3.x;
|
||||
p43.y = p4.y - p3.y;
|
||||
p43.z = p4.z - p3.z;
|
||||
// Disabled - just checks that P3P4 isn't length 0, which it won't be.
|
||||
#if 0
|
||||
if (ABS(p43.x) < FLT_EPSILON && ABS(p43.y) < FLT_EPSILON && ABS(p43.z) < FLT_EPSILON)
|
||||
return false;
|
||||
#endif
|
||||
p21.x = p2.x - p1.x;
|
||||
p21.y = p2.y - p1.y;
|
||||
p21.z = p2.z - p1.z;
|
||||
|
||||
// Ditto, checks that P2P1 isn't length 0.
|
||||
#if 0
|
||||
if (ABS(p21.x) < EPS && ABS(p21.y) < EPS && ABS(p21.z) < EPS)
|
||||
return false;
|
||||
#endif
|
||||
|
||||
d1343 = p13.x * p43.x + p13.y * p43.y + p13.z * p43.z;
|
||||
d4321 = p43.x * p21.x + p43.y * p21.y + p43.z * p21.z;
|
||||
d1321 = p13.x * p21.x + p13.y * p21.y + p13.z * p21.z;
|
||||
d4343 = p43.x * p43.x + p43.y * p43.y + p43.z * p43.z;
|
||||
d2121 = p21.x * p21.x + p21.y * p21.y + p21.z * p21.z;
|
||||
|
||||
denom = d2121 * d4343 - d4321 * d4321;
|
||||
|
||||
// Division-by-zero check
|
||||
if (fabsf(denom) < FLT_EPSILON) {
|
||||
return false;
|
||||
}
|
||||
number = d1343 * d4321 - d1321 * d4343;
|
||||
|
||||
*mua = number / denom;
|
||||
*mub = (d1343 + d4321 * (*mua)) / d4343;
|
||||
|
||||
pa->x = p1.x + *mua * p21.x;
|
||||
pa->y = p1.y + *mua * p21.y;
|
||||
pa->z = p1.z + *mua * p21.z;
|
||||
pb->x = p3.x + *mub * p43.x;
|
||||
pb->y = p3.y + *mub * p43.y;
|
||||
pb->z = p3.z + *mub * p43.z;
|
||||
|
||||
return (true);
|
||||
}
|
Loading…
Reference in a new issue