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:
Moses Turner 2022-12-29 13:56:30 -06:00 committed by Moses Turner
parent 7da72f4697
commit 946141ce40
10 changed files with 26 additions and 1154 deletions

View file

@ -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}

View file

@ -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");

View file

@ -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;

View file

@ -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})

View file

@ -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

View file

@ -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

View file

@ -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

View file

@ -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

View file

@ -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

View file

@ -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);
}