mirrors/monado
1
0
Fork 0
mirror of https://gitlab.freedesktop.org/monado/monado.git synced 2024-12-28 02:26:16 +00:00
Code Issues Actions Packages Projects Releases Wiki Activity

h/mercury: Add Levenberg-Marquardt optimizer, and lots of fixes!

Browse source 
Co-authored-by: Charlton Rodda <charlton.rodda@collabora.com>
Co-authored-by: Ryan Pavlik <ryan.pavlik@collabora.com>
This commit is contained in:
Moses Turner 2022-06-20 16:33:37 +01:00
parent 8040224b39
commit 73dbc712ab
21 changed files with 3413 additions and 634 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

65
src/xrt/tracking/hand/mercury/CMakeLists.txt
View file

@ -1,15 +1,53 @@
# Copyright 2019-2022, Collabora, Ltd.
# SPDX-License-Identifier: BSL-1.0
# Mercury hand tracking library!
add_library(t_ht_mercury_kine STATIC kine/kinematic_interface.hpp kine/kinematic_main.cpp)
add_subdirectory(kine_lm)
add_subdirectory(kine_ccdik)
target_link_libraries(t_ht_mercury_kine PRIVATE aux_math aux_tracking aux_os aux_util)
add_library(
t_ht_mercury_model STATIC
hg_model.cpp
)
target_link_libraries(
t_ht_mercury_model
PRIVATE
aux_math
aux_tracking
aux_os
aux_util
)
target_include_directories(t_ht_mercury_model
SYSTEM PRIVATE
${OpenCV_INCLUDE_DIRS}
${EIGEN3_INCLUDE_DIR}
)
target_link_libraries(
t_ht_mercury_model
PRIVATE
aux_math
aux_tracking
aux_os
aux_util
${OpenCV_LIBRARIES}
ONNXRuntime::ONNXRuntime
)
add_library(
t_ht_mercury STATIC
hg_sync.cpp
hg_sync.hpp
hg_interface.h
kine_common.hpp
)
target_include_directories(t_ht_mercury_kine SYSTEM PRIVATE ${EIGEN3_INCLUDE_DIR})
add_library(t_ht_mercury STATIC hg_interface.h hg_model.hpp hg_sync.cpp hg_sync.hpp)
target_link_libraries(
t_ht_mercury
PUBLIC aux-includes xrt-external-cjson
@ -19,13 +57,24 @@ target_link_libraries(
aux_os
aux_util
ONNXRuntime::ONNXRuntime
t_ht_mercury_kine
t_ht_mercury_kine_lm
t_ht_mercury_kine_ccdik
t_ht_mercury_model
# ncnn # Soon...
${OpenCV_LIBRARIES}
)
if(XRT_HAVE_OPENCV)
target_include_directories(
t_ht_mercury SYSTEM PRIVATE ${OpenCV_INCLUDE_DIRS} ${EIGEN3_INCLUDE_DIR}
)
target_include_directories(t_ht_mercury SYSTEM PRIVATE ${OpenCV_INCLUDE_DIRS} ${EIGEN3_INCLUDE_DIR})
target_link_libraries(t_ht_mercury PUBLIC ${OpenCV_LIBRARIES})
endif()
# Below is entirely just so that tests can find us.
add_library(
t_ht_mercury_includes INTERFACE
)
target_include_directories(
t_ht_mercury_includes INTERFACE ${CMAKE_CURRENT_SOURCE_DIR} ${CMAKE_CURRENT_BINARY_DIR}
)

174
src/xrt/tracking/hand/mercury/hg_image_math.hpp
View file

@ -1,174 +0,0 @@
// Copyright 2021-2022, Collabora, Ltd.
// SPDX-License-Identifier: BSL-1.0
/*!
* @file
* @brief Helper header for drawing and image transforms
* @author Moses Turner <moses@collabora.com>
* @ingroup tracking
*/
#pragma once
#include "hg_sync.hpp"
namespace xrt::tracking::hand::mercury {
/*!
* This is a template so that we can use xrt_vec3 or xrt_vec2.
* Please don't use this for anything other than xrt_vec3 or xrt_vec2!
*/
template <typename T>
T
transformVecBy2x3(T in, cv::Matx23f warp_back)
{
T rr;
rr.x = (in.x * warp_back(0, 0)) + (in.y * warp_back(0, 1)) + warp_back(0, 2);
rr.y = (in.x * warp_back(1, 0)) + (in.y * warp_back(1, 1)) + warp_back(1, 2);
return rr;
}
cv::Scalar
hsv2rgb(float fH, float fS, float fV)
{
const float fC = fV * fS; // Chroma
const float fHPrime = fmod(fH / 60.0, 6);
const float fX = fC * (1 - fabs(fmod(fHPrime, 2) - 1));
const float fM = fV - fC;
float fR, fG, fB;
if (0 <= fHPrime && fHPrime < 1) {
fR = fC;
fG = fX;
fB = 0;
} else if (1 <= fHPrime && fHPrime < 2) {
fR = fX;
fG = fC;
fB = 0;
} else if (2 <= fHPrime && fHPrime < 3) {
fR = 0;
fG = fC;
fB = fX;
} else if (3 <= fHPrime && fHPrime < 4) {
fR = 0;
fG = fX;
fB = fC;
} else if (4 <= fHPrime && fHPrime < 5) {
fR = fX;
fG = 0;
fB = fC;
} else if (5 <= fHPrime && fHPrime < 6) {
fR = fC;
fG = 0;
fB = fX;
} else {
fR = 0;
fG = 0;
fB = 0;
}
fR += fM;
fG += fM;
fB += fM;
return {fR * 255.0f, fG * 255.0f, fB * 255.0f};
}
struct xrt_vec2
raycoord(ht_view *htv, struct xrt_vec2 model_out)
{
model_out.x *= htv->hgt->multiply_px_coord_for_undistort;
model_out.y *= htv->hgt->multiply_px_coord_for_undistort;
cv::Mat in_px_coords(1, 1, CV_32FC2);
float *write_in;
write_in = in_px_coords.ptr<float>(0);
write_in[0] = model_out.x;
write_in[1] = model_out.y;
cv::Mat out_ray(1, 1, CV_32FC2);
if (htv->hgt->use_fisheye) {
cv::fisheye::undistortPoints(in_px_coords, out_ray, htv->cameraMatrix, htv->distortion);
} else {
cv::undistortPoints(in_px_coords, out_ray, htv->cameraMatrix, htv->distortion);
}
float n_x = out_ray.at<float>(0, 0);
float n_y = out_ray.at<float>(0, 1);
struct xrt_vec3 n = {n_x, n_y, 1.0f};
cv::Matx33f R = htv->rotate_camera_to_stereo_camera;
struct xrt_vec3 o = {
(n.x * R(0, 0)) + (n.y * R(0, 1)) + (n.z * R(0, 2)),
(n.x * R(1, 0)) + (n.y * R(1, 1)) + (n.z * R(1, 2)),
(n.x * R(2, 0)) + (n.y * R(2, 1)) + (n.z * R(2, 2)),
};
math_vec3_scalar_mul(1.0f / o.z, &o);
return {o.x, o.y};
}
cv::Matx23f
blackbar(const cv::Mat &in, cv::Mat &out, xrt_size out_size)
{
#if 1
// Easy to think about, always right, but pretty slow:
// Get a matrix from the original to the scaled down / blackbar'd image, then get one that goes back.
// Then just warpAffine() it.
// Easy in programmer time - never have to worry about off by one, special cases. We can come back and optimize
// later.
// Do the black bars need to be on top and bottom, or on left and right?
float scale_down_w = (float)out_size.w / (float)in.cols; // 128/1280 = 0.1
float scale_down_h = (float)out_size.h / (float)in.rows; // 128/800 = 0.16
float scale_down = fmin(scale_down_w, scale_down_h); // 0.1
float width_inside = (float)in.cols * scale_down;
float height_inside = (float)in.rows * scale_down;
float translate_x = (out_size.w - width_inside) / 2; // should be 0 for 1280x800
float translate_y = (out_size.h - height_inside) / 2; // should be (1280-800)/2 = 240
cv::Matx23f go;
// clang-format off
go(0,0) = scale_down; go(0,1) = 0.0f; go(0,2) = translate_x;
go(1,0) = 0.0f; go(1,1) = scale_down; go(1,2) = translate_y;
// clang-format on
cv::warpAffine(in, out, go, cv::Size(out_size.w, out_size.h));
cv::Matx23f ret;
// clang-format off
ret(0,0) = 1.0f/scale_down; ret(0,1) = 0.0f; ret(0,2) = -translate_x/scale_down;
ret(1,0) = 0.0f; ret(1,1) = 1.0f/scale_down; ret(1,2) = -translate_y/scale_down;
// clang-format on
return ret;
#else
// Fast, always wrong if the input isn't square. You'd end up using something like this, plus some
// copyMakeBorder if you want to optimize.
if (aspect_ratio_input == aspect_ratio_output) {
cv::resize(in, out, {out_size.w, out_size.h});
cv::Matx23f ret;
float scale_from_out_to_in = (float)in.cols / (float)out_size.w;
// clang-format off
ret(0,0) = scale_from_out_to_in; ret(0,1) = 0.0f; ret(0,2) = 0.0f;
ret(1,0) = 0.0f; ret(1,1) = scale_from_out_to_in; ret(1,2) = 0.0f;
// clang-format on
cv::imshow("hi", out);
cv::waitKey(1);
return ret;
}
assert(!"Uh oh! Unimplemented!");
return {};
#endif
}
void
handDot(cv::Mat &mat, xrt_vec2 place, float radius, float hue, float intensity, int type)
{
cv::circle(mat, {(int)place.x, (int)place.y}, radius, hsv2rgb(hue * 360.0f, intensity, intensity), type);
}
} // namespace xrt::tracking::hand::mercury

109
src/xrt/tracking/hand/mercury/hg_image_math.inl Normal file
View file

@ -0,0 +1,109 @@
// Copyright 2021-2022, Collabora, Ltd.
// SPDX-License-Identifier: BSL-1.0
/*!
* @file
* @brief Helper header for drawing and image transforms
* @author Moses Turner <moses@collabora.com>
* @ingroup tracking
*/
#pragma once
#include "hg_sync.hpp"
namespace xrt::tracking::hand::mercury {
/*!
* This is a template so that we can use xrt_vec3 or xrt_vec2.
* Please don't use this for anything other than xrt_vec3 or xrt_vec2!
*/
template <typename T>
T
transformVecBy2x3(T in, cv::Matx23f warp_back)
{
T rr;
rr.x = (in.x * warp_back(0, 0)) + (in.y * warp_back(0, 1)) + warp_back(0, 2);
rr.y = (in.x * warp_back(1, 0)) + (in.y * warp_back(1, 1)) + warp_back(1, 2);
return rr;
}
static cv::Scalar
hsv2rgb(float fH, float fS, float fV)
{
const float fC = fV * fS; // Chroma
const float fHPrime = fmod(fH / 60.0, 6);
const float fX = fC * (1 - fabs(fmod(fHPrime, 2) - 1));
const float fM = fV - fC;
float fR, fG, fB;
if (0 <= fHPrime && fHPrime < 1) {
fR = fC;
fG = fX;
fB = 0;
} else if (1 <= fHPrime && fHPrime < 2) {
fR = fX;
fG = fC;
fB = 0;
} else if (2 <= fHPrime && fHPrime < 3) {
fR = 0;
fG = fC;
fB = fX;
} else if (3 <= fHPrime && fHPrime < 4) {
fR = 0;
fG = fX;
fB = fC;
} else if (4 <= fHPrime && fHPrime < 5) {
fR = fX;
fG = 0;
fB = fC;
} else if (5 <= fHPrime && fHPrime < 6) {
fR = fC;
fG = 0;
fB = fX;
} else {
fR = 0;
fG = 0;
fB = 0;
}
fR += fM;
fG += fM;
fB += fM;
return {fR * 255.0f, fG * 255.0f, fB * 255.0f};
}
//! @optimize Make it take an array of vec2's and give out an array of vec2's, then
// put it in its own target so we don't have to link to OpenCV.
// Oooorrrrr... actually add good undistortion stuff to Monado so that we don't have to depend on OpenCV at all.
XRT_MAYBE_UNUSED static struct xrt_vec2
raycoord(ht_view *htv, struct xrt_vec2 model_out)
{
model_out.x *= htv->hgt->multiply_px_coord_for_undistort;
model_out.y *= htv->hgt->multiply_px_coord_for_undistort;
cv::Mat in_px_coords(1, 1, CV_32FC2);
float *write_in;
write_in = in_px_coords.ptr<float>(0);
write_in[0] = model_out.x;
write_in[1] = model_out.y;
cv::Mat out_ray(1, 1, CV_32FC2);
if (htv->hgt->use_fisheye) {
cv::fisheye::undistortPoints(in_px_coords, out_ray, htv->cameraMatrix, htv->distortion);
} else {
cv::undistortPoints(in_px_coords, out_ray, htv->cameraMatrix, htv->distortion);
}
float n_x = out_ray.at<float>(0, 0);
float n_y = out_ray.at<float>(0, 1);
return {n_x, n_y};
}
static void
handDot(cv::Mat &mat, xrt_vec2 place, float radius, float hue, float intensity, int type)
{
cv::circle(mat, {(int)place.x, (int)place.y}, radius, hsv2rgb(hue * 360.0f, intensity, intensity), type);
}
} // namespace xrt::tracking::hand::mercury

139
src/xrt/tracking/hand/mercury/hg_model.hpp → src/xrt/tracking/hand/mercury/hg_model.cpp
View file

@ -9,10 +9,8 @@
// Many C api things were stolen from here (MIT license):
// https://github.com/microsoft/onnxruntime-inference-examples/blob/main/c_cxx/fns_candy_style_transfer/fns_candy_style_transfer.c
#pragma once
#include "hg_sync.hpp"
#include "hg_image_math.hpp"
#include "hg_image_math.inl"
#include <filesystem>
@ -20,11 +18,6 @@
namespace xrt::tracking::hand::mercury {
cv::Scalar RED(255, 30, 30);
cv::Scalar YELLOW(255, 255, 0);
cv::Scalar colors[2] = {YELLOW, RED};
#define ORT(expr) \
do { \
OrtStatus *status = wrap->api->expr; \
@ -37,22 +30,80 @@ cv::Scalar colors[2] = {YELLOW, RED};
} while (0)
static bool
argmax(const float *data, int size, int *out_idx)
static cv::Matx23f
blackbar(const cv::Mat &in, cv::Mat &out, xrt_size out_size)
{
float max_value = -1.0f;
bool found = false;
for (int i = 0; i < size; i++) {
if (data[i] > max_value) {
max_value = data[i];
*out_idx = i;
found = true;
}
#if 1
// Easy to think about, always right, but pretty slow:
// Get a matrix from the original to the scaled down / blackbar'd image, then get one that goes back.
// Then just warpAffine() it.
// Easy in programmer time - never have to worry about off by one, special cases. We can come back and optimize
// later.
// Do the black bars need to be on top and bottom, or on left and right?
float scale_down_w = (float)out_size.w / (float)in.cols; // 128/1280 = 0.1
float scale_down_h = (float)out_size.h / (float)in.rows; // 128/800 = 0.16
float scale_down = fmin(scale_down_w, scale_down_h); // 0.1
float width_inside = (float)in.cols * scale_down;
float height_inside = (float)in.rows * scale_down;
float translate_x = (out_size.w - width_inside) / 2; // should be 0 for 1280x800
float translate_y = (out_size.h - height_inside) / 2; // should be (1280-800)/2 = 240
cv::Matx23f go;
// clang-format off
go(0,0) = scale_down; go(0,1) = 0.0f; go(0,2) = translate_x;
go(1,0) = 0.0f; go(1,1) = scale_down; go(1,2) = translate_y;
// clang-format on
cv::warpAffine(in, out, go, cv::Size(out_size.w, out_size.h));
cv::Matx23f ret;
// clang-format off
ret(0,0) = 1.0f/scale_down; ret(0,1) = 0.0f; ret(0,2) = -translate_x/scale_down;
ret(1,0) = 0.0f; ret(1,1) = 1.0f/scale_down; ret(1,2) = -translate_y/scale_down;
// clang-format on
return ret;
#else
// Fast, always wrong if the input isn't square. You'd end up using something like this, plus some
// copyMakeBorder if you want to optimize.
if (aspect_ratio_input == aspect_ratio_output) {
cv::resize(in, out, {out_size.w, out_size.h});
cv::Matx23f ret;
float scale_from_out_to_in = (float)in.cols / (float)out_size.w;
// clang-format off
ret(0,0) = scale_from_out_to_in; ret(0,1) = 0.0f; ret(0,2) = 0.0f;
ret(1,0) = 0.0f; ret(1,1) = scale_from_out_to_in; ret(1,2) = 0.0f;
// clang-format on
cv::imshow("hi", out);
cv::waitKey(1);
return ret;
}
return found;
assert(!"Uh oh! Unimplemented!");
return {};
#endif
}
void
static inline int
argmax(const float *data, int size)
{
float max_value = data[0];
int out_idx = 0;
for (int i = 1; i < size; i++) {
if (data[i] > max_value) {
max_value = data[i];
out_idx = i;
}
}
return out_idx;
}
static void
refine_center_of_distribution(
const float *data, int coarse_x, int coarse_y, int w, int h, float *out_refined_x, float *out_refined_y)
{
@ -139,6 +190,11 @@ normalizeGrayscaleImage(cv::Mat &data_in, cv::Mat &data_out)
cv::Mat stddev;
cv::meanStdDev(data_out, mean, stddev);
if (stddev.at<double>(0, 0) == 0) {
U_LOG_W("Got image with zero standard deviation!");
return;
}
data_out *= 0.25 / stddev.at<double>(0, 0);
// Calculate it again; mean has changed. Yes we don't need to but it's easy
@ -147,7 +203,7 @@ normalizeGrayscaleImage(cv::Mat &data_in, cv::Mat &data_out)
data_out += (0.5 - mean.at<double>(0, 0));
}
static void
void
init_hand_detection(HandTracking *hgt, onnx_wrap *wrap)
{
std::filesystem::path path = hgt->models_folder;
@ -203,11 +259,16 @@ run_hand_detection(void *ptr)
{
XRT_TRACE_MARKER();
ht_view *view = (ht_view *)ptr;
// ht_view *view = (ht_view *)ptr;
hand_detection_run_info *info = (hand_detection_run_info *)ptr;
ht_view *view = info->view;
HandTracking *hgt = view->hgt;
onnx_wrap *wrap = &view->detection;
cv::Mat &data_400x640 = view->run_model_on_this;
cv::Mat _240x320_uint8;
xrt_size desire;
@ -234,13 +295,14 @@ run_hand_detection(void *ptr)
for (int hand_idx = 0; hand_idx < 2; hand_idx++) {
const float *this_side_data = out_data + hand_idx * plane_size * 2;
int max_idx;
int max_idx = argmax(this_side_data, 4800);
det_output *output = &view->det_outputs[hand_idx];
hand_bounding_box *output = info->outputs[hand_idx];
output->found = argmax(this_side_data, 4800, &max_idx) && this_side_data[max_idx] > 0.3;
output->found = this_side_data[max_idx] > 0.3;
if (output->found) {
output->confidence = this_side_data[max_idx];
int row = max_idx / 80;
int col = max_idx % 80;
@ -268,7 +330,7 @@ run_hand_detection(void *ptr)
cv::Point2i pt(_pt.x, _pt.y);
cv::rectangle(debug_frame,
cv::Rect(pt - cv::Point2i(size / 2, size / 2), cv::Size(size, size)),
colors[hand_idx], 1);
PINK, 1);
}
}
}
@ -276,7 +338,7 @@ run_hand_detection(void *ptr)
wrap->api->ReleaseValue(output_tensor);
}
static void
void
init_keypoint_estimation(HandTracking *hgt, onnx_wrap *wrap)
{
@ -341,7 +403,7 @@ run_keypoint_estimation(void *ptr)
cv::Mat &debug = info->view->debug_out_to_this;
det_output *output = &info->view->det_outputs[info->hand_idx];
hand_bounding_box *output = &info->view->bboxes_this_frame[info->hand_idx];
cv::Point2f src_tri[3];
cv::Point2f dst_tri[3];
@ -402,10 +464,8 @@ run_keypoint_estimation(void *ptr)
cv::Mat y(cv::Size(128, 21), CV_32FC1, out_data_y);
for (int i = 0; i < 21; i++) {
int loc_x;
int loc_y;
argmax(&out_data_x[i * 128], 128, &loc_x);
argmax(&out_data_y[i * 128], 128, &loc_y);
int loc_x = argmax(&out_data_x[i * 128], 128);
int loc_y = argmax(&out_data_y[i * 128], 128);
xrt_vec2 loc;
loc.x = loc_x;
loc.y = loc_y;
@ -415,7 +475,7 @@ run_keypoint_estimation(void *ptr)
tan_space.kps[i] = raycoord(info->view, loc);
}
if (hgt->debug_scribble) {
if (hgt->debug_scribble && hgt->tuneable_values.scribble_keypoint_model_outputs) {
for (int finger = 0; finger < 5; finger++) {
cv::Point last = {(int)keypoints_global[0].x, (int)keypoints_global[0].y};
for (int joint = 0; joint < 4; joint++) {
@ -438,7 +498,7 @@ run_keypoint_estimation(void *ptr)
}
static void
void
init_keypoint_estimation_new(HandTracking *hgt, onnx_wrap *wrap)
{
@ -508,7 +568,7 @@ run_keypoint_estimation_new(void *ptr)
cv::Mat &debug = info->view->debug_out_to_this;
det_output *output = &info->view->det_outputs[info->hand_idx];
hand_bounding_box *output = &info->view->bboxes_this_frame[info->hand_idx];
cv::Point2f src_tri[3];
cv::Point2f dst_tri[3];
@ -570,31 +630,34 @@ run_keypoint_estimation_new(void *ptr)
Hand2D &px_coord = info->view->keypoint_outputs[info->hand_idx].hand_px_coord;
Hand2D &tan_space = info->view->keypoint_outputs[info->hand_idx].hand_tan_space;
float *confidences = info->view->keypoint_outputs[info->hand_idx].hand_tan_space.confidences;
xrt_vec2 *keypoints_global = px_coord.kps;
size_t plane_size = 22 * 22;
for (int i = 0; i < 21; i++) {
float *data = &out_data[i * plane_size];
int out_idx = 0;
argmax(data, 22 * 22, &out_idx);
int out_idx = argmax(data, 22 * 22);
int row = out_idx / 22;
int col = out_idx % 22;
xrt_vec2 loc;
refine_center_of_distribution(data, col, row, 22, 22, &loc.x, &loc.y);
loc.x *= 128.0f / 22.0f;
loc.y *= 128.0f / 22.0f;
loc = transformVecBy2x3(loc, go_back);
confidences[i] = data[out_idx];
px_coord.kps[i] = loc;
tan_space.kps[i] = raycoord(info->view, loc);
}
if (hgt->debug_scribble) {
if (hgt->debug_scribble && hgt->tuneable_values.scribble_keypoint_model_outputs) {
for (int finger = 0; finger < 5; finger++) {
cv::Point last = {(int)keypoints_global[0].x, (int)keypoints_global[0].y};
for (int joint = 0; joint < 4; joint++) {

865
src/xrt/tracking/hand/mercury/hg_sync.cpp
View file

File diff suppressed because it is too large Load diff

149
src/xrt/tracking/hand/mercury/hg_sync.hpp
View file

@ -14,6 +14,7 @@
#include "tracking/t_calibration_opencv.hpp"
#include "tracking/t_hand_tracking.h"
#include "xrt/xrt_defines.h"
#include "xrt/xrt_frame.h"
@ -32,6 +33,8 @@
#include "util/u_frame.h"
#include "util/u_var.h"
#include "util/u_template_historybuf.hpp"
#include <assert.h>
#include <stdio.h>
#include <stdlib.h>
@ -41,62 +44,40 @@
#include <opencv2/opencv.hpp>
#include <onnxruntime_c_api.h>
#include "kine/kinematic_interface.hpp"
#include "kine_common.hpp"
#include "kine_lm/lm_interface.hpp"
#include "kine_ccdik/ccdik_interface.hpp"
namespace xrt::tracking::hand::mercury {
using namespace xrt::auxiliary::util;
DEBUG_GET_ONCE_LOG_OPTION(mercury_log, "MERCURY_LOG", U_LOGGING_WARN)
DEBUG_GET_ONCE_BOOL_OPTION(mercury_use_simdr_keypoint, "MERCURY_USE_SIMDR_KEYPOINT", false)
#define HG_TRACE(hgt, ...) U_LOG_IFL_T(hgt->log_level, __VA_ARGS__)
#define HG_DEBUG(hgt, ...) U_LOG_IFL_D(hgt->log_level, __VA_ARGS__)
#define HG_INFO(hgt, ...) U_LOG_IFL_I(hgt->log_level, __VA_ARGS__)
#define HG_WARN(hgt, ...) U_LOG_IFL_W(hgt->log_level, __VA_ARGS__)
#define HG_ERROR(hgt, ...) U_LOG_IFL_E(hgt->log_level, __VA_ARGS__)
static const cv::Scalar RED(255, 30, 30);
static const cv::Scalar YELLOW(255, 255, 0);
static const cv::Scalar PINK(255, 0, 255);
static const cv::Scalar colors[2] = {YELLOW, RED};
#undef USE_NCNN
// Forward declaration for ht_view
struct HandTracking;
struct ht_view;
enum Joint21
{
WRIST = 0,
THMB_MCP = 1,
THMB_PXM = 2,
THMB_DST = 3,
THMB_TIP = 4,
INDX_PXM = 5,
INDX_INT = 6,
INDX_DST = 7,
INDX_TIP = 8,
MIDL_PXM = 9,
MIDL_INT = 10,
MIDL_DST = 11,
MIDL_TIP = 12,
RING_PXM = 13,
RING_INT = 14,
RING_DST = 15,
RING_TIP = 16,
LITL_PXM = 17,
LITL_INT = 18,
LITL_DST = 19,
LITL_TIP = 20
};
// Using the compiler to stop me from getting 2D space mixed up with 3D space.
struct Hand2D
{
struct xrt_vec2 kps[21];
float confidences[21];
};
struct Hand3D
@ -117,17 +98,26 @@ struct onnx_wrap
const char *input_name;
};
struct det_output
struct hand_bounding_box
{
xrt_vec2 center;
float size_px;
bool found;
bool confidence;
};
struct hand_detection_run_info
{
ht_view *view;
hand_bounding_box *outputs[2];
};
struct keypoint_output
{
Hand2D hand_px_coord;
Hand2D hand_tan_space;
float confidences[21];
};
struct keypoint_estimation_run_info
@ -150,12 +140,36 @@ struct ht_view
cv::Mat run_model_on_this;
cv::Mat debug_out_to_this;
struct det_output det_outputs[2]; // left, right
struct hand_bounding_box bboxes_this_frame[2]; // left, right
struct keypoint_estimation_run_info run_info[2];
struct keypoint_output keypoint_outputs[2];
};
struct hand_history
{
HistoryBuffer<xrt_hand_joint_set, 5> hands;
HistoryBuffer<uint64_t, 5> timestamps;
};
struct output_struct_one_frame
{
xrt_vec2 left[21];
float confidences_left[21];
xrt_vec2 right[21];
float confidences_right[21];
};
struct hand_size_refinement
{
int num_hands;
float out_hand_size;
float out_hand_confidence;
float hand_size_refinement_schedule_x = 0;
float hand_size_refinement_schedule_y = 0;
};
/*!
* Main class of Mercury hand tracking.
*
@ -193,10 +207,9 @@ public:
u_worker_group *group;
float hand_size;
float baseline = {};
struct xrt_quat stereo_camera_to_left_camera = {};
xrt_pose hand_pose_camera_offset = {};
uint64_t current_frame_timestamp = {};
@ -207,15 +220,49 @@ public:
enum u_logging_level log_level = U_LOGGING_INFO;
kine::kinematic_hand_4f *kinematic_hands[2];
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};
bool last_frame_hand_detected[2] = {false, false};
bool this_frame_hand_detected[2] = {false, false};
struct hand_history histories[2];
int detection_counter = 0;
struct hand_size_refinement refinement = {};
// Moses hand size is ~0.095; they has big-ish hands so let's do 0.09
float target_hand_size = 0.09;
xrt_frame *debug_frame;
void (*keypoint_estimation_run_func)(void *);
struct xrt_pose left_in_right = {};
struct hand_tracking_image_boundary_info image_boundary_info;
u_frame_times_widget ft_widget = {};
struct
{
bool new_user_event = false;
struct u_var_draggable_f32 dyn_radii_fac;
struct u_var_draggable_f32 dyn_joint_y_angle_error;
struct u_var_draggable_f32 amount_to_lerp_prediction;
bool scribble_predictions_into_this_frame = false;
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;
public:
explicit HandTracking();
~HandTracking();
@ -238,4 +285,28 @@ public:
cCallbackDestroy(t_hand_tracking_sync *ht_sync);
};
void
init_hand_detection(HandTracking *hgt, onnx_wrap *wrap);
void
run_hand_detection(void *ptr);
void
init_keypoint_estimation(HandTracking *hgt, onnx_wrap *wrap);
void
run_keypoint_estimation(void *ptr);
void
init_keypoint_estimation_new(HandTracking *hgt, onnx_wrap *wrap);
void
run_keypoint_estimation_new(void *ptr);
void
release_onnx_wrap(onnx_wrap *wrap);
} // namespace xrt::tracking::hand::mercury

34
src/xrt/tracking/hand/mercury/kine/kinematic_interface.hpp
View file

@ -1,34 +0,0 @@
// Copyright 2022, Collabora, Ltd.
// SPDX-License-Identifier: BSL-1.0
/*!
* @file
* @brief Interface for kinematic model
* @author Moses Turner <moses@collabora.com>
* @ingroup tracking
*/
#pragma once
#include "math/m_api.h"
#include "xrt/xrt_defines.h"
namespace xrt::tracking::hand::mercury::kine {
struct kinematic_hand_4f;
void
alloc_kinematic_hand(kinematic_hand_4f **out_kinematic_hand);
void
optimize_new_frame(xrt_vec3 model_joints_3d[21],
kinematic_hand_4f *hand,
struct xrt_hand_joint_set *out_set,
int hand_idx);
void
init_hardcoded_statics(kinematic_hand_4f *hand, float size = 0.095864);
void
free_kinematic_hand(kinematic_hand_4f **kinematic_hand);
} // namespace xrt::tracking::hand::mercury::kine

20
src/xrt/tracking/hand/mercury/kine_ccdik/CMakeLists.txt Normal file
View file

@ -0,0 +1,20 @@
# 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})

89
src/xrt/tracking/hand/mercury/kine/kinematic_defines.hpp → src/xrt/tracking/hand/mercury/kine_ccdik/ccdik_defines.hpp
View file

@ -35,44 +35,12 @@
#include <stddef.h>
#include <unistd.h>
#include "../kine_common.hpp"
using namespace xrt::auxiliary::math;
namespace xrt::tracking::hand::mercury::kine {
// Not doing enum class, I *want* to allow implicit conversions.
namespace Joint21 {
enum Joint21
{
WRIST = 0,
THMB_MCP = 1,
THMB_PXM = 2,
THMB_DST = 3,
THMB_TIP = 4,
INDX_PXM = 5,
INDX_INT = 6,
INDX_DST = 7,
INDX_TIP = 8,
MIDL_PXM = 9,
MIDL_INT = 10,
MIDL_DST = 11,
MIDL_TIP = 12,
RING_PXM = 13,
RING_INT = 14,
RING_DST = 15,
RING_TIP = 16,
LITL_PXM = 17,
LITL_INT = 18,
LITL_DST = 19,
LITL_TIP = 20
};
}
namespace xrt::tracking::hand::mercury::ccdik {
enum class HandJoint26KP : int
{
@ -135,26 +103,30 @@ enum ThumbBone
TB_DISTAL
};
const size_t kNumNNJoints = 21;
struct wct_t
{
float waggle;
float curl;
float twist;
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;
wct_t rot_to_next_joint_wct;
Eigen::Quaternionf rot_to_next_joint_quat;
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;
Eigen::Affine3f bone_relation = {};
// Imagine it like transforming an object at the origin to this bone's position/orientation.
Eigen::Affine3f world_pose;
Eigen::Affine3f world_pose = {};
struct
{
@ -163,15 +135,12 @@ struct bone_t
} parent;
wct_t joint_limit_min;
wct_t joint_limit_max;
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;
// float static_weight
// float model_confidence_weight ?
Joint21::Joint21 keypoint_idx_21 = {};
};
// translation: wrist to mcp (-z and x). rotation: from wrist space to metacarpal space
@ -179,28 +148,26 @@ struct bone_t
struct finger_t
{
bone_t bones[5];
bone_t bones[5] = {};
};
typedef struct kinematic_hand_4f
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;
Eigen::Affine3f wrist_relation = {};
finger_t fingers[5];
finger_t fingers[5] = {};
xrt_vec3 t_jts[21];
Eigen::Matrix<float, 3, 21> t_jts_as_mat;
Eigen::Matrix<float, 3, 21> kinematic;
// // Pointers to the locations of
// struct xrt_vec3 *loc_ptrs[21];
} kinematic_hand_4f;
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 \
@ -208,4 +175,4 @@ typedef struct kinematic_hand_4f
continue; \
}
} // namespace xrt::tracking::hand::mercury::kine
} // namespace xrt::tracking::hand::mercury::ccdik

22
src/xrt/tracking/hand/mercury/kine/kinematic_hand_init.hpp → src/xrt/tracking/hand/mercury/kine_ccdik/ccdik_hand_init.hpp
View file

@ -10,11 +10,11 @@
#pragma once
#include "kinematic_defines.hpp"
#include "kinematic_tiny_math.hpp"
#include "ccdik_defines.hpp"
#include "ccdik_tiny_math.hpp"
namespace xrt::tracking::hand::mercury::kine {
namespace xrt::tracking::hand::mercury::ccdik {
void
bone_update_quat_and_matrix(struct bone_t *bone)
@ -39,7 +39,7 @@ eval_chain(std::vector<const Eigen::Affine3f *> &chain, Eigen::Affine3f &out)
}
void
_statics_init_world_parents(kinematic_hand_4f *hand)
_statics_init_world_parents(KinematicHandCCDIK *hand)
{
for (int finger = 0; finger < 5; finger++) {
finger_t *of = &hand->fingers[finger];
@ -56,7 +56,7 @@ _statics_init_world_parents(kinematic_hand_4f *hand)
}
void
_statics_init_world_poses(kinematic_hand_4f *hand)
_statics_init_world_poses(KinematicHandCCDIK *hand)
{
XRT_TRACE_MARKER();
for (int finger = 0; finger < 5; finger++) {
@ -70,7 +70,7 @@ _statics_init_world_poses(kinematic_hand_4f *hand)
}
void
_statics_init_loc_ptrs(kinematic_hand_4f *hand)
_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;
@ -99,7 +99,7 @@ _statics_init_loc_ptrs(kinematic_hand_4f *hand)
}
void
_statics_joint_limits(kinematic_hand_4f *hand)
_statics_joint_limits(KinematicHandCCDIK *hand)
{
{
finger_t *t = &hand->fingers[0];
@ -138,9 +138,8 @@ _statics_joint_limits(kinematic_hand_4f *hand)
// Exported:
void
init_hardcoded_statics(kinematic_hand_4f *hand, float size)
init_hardcoded_statics(KinematicHandCCDIK *hand, float size)
{
memset(hand, 0, sizeof(kinematic_hand_4f));
hand->size = size;
hand->wrist_relation.setIdentity();
hand->wrist_relation.linear() *= hand->size;
@ -202,10 +201,13 @@ init_hardcoded_statics(kinematic_hand_4f *hand, float size)
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;
@ -227,4 +229,4 @@ init_hardcoded_statics(kinematic_hand_4f *hand, float size)
_statics_init_loc_ptrs(hand);
_statics_joint_limits(hand);
}
} // namespace xrt::tracking::hand::mercury::kine
} // namespace xrt::tracking::hand::mercury::ccdik

31
src/xrt/tracking/hand/mercury/kine_ccdik/ccdik_interface.hpp Normal file
View file

@ -0,0 +1,31 @@
// 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

204
src/xrt/tracking/hand/mercury/kine/kinematic_main.cpp → src/xrt/tracking/hand/mercury/kine_ccdik/ccdik_main.cpp
View file

@ -7,9 +7,12 @@
* @ingroup tracking
*/
#include "kinematic_defines.hpp"
#include "kinematic_tiny_math.hpp"
#include "kinematic_hand_init.hpp"
#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>
@ -18,17 +21,17 @@
namespace xrt::tracking::hand::mercury::kine {
namespace xrt::tracking::hand::mercury::ccdik {
static void
_two_set_ele(Eigen::Matrix<float, 3, 21> &thing, Eigen::Affine3f jt, int idx)
_two_set_ele(Eigen::Matrix<float, 3, kNumNNJoints> &thing, Eigen::Affine3f jt, int idx)
{
// slow
//! @optimize
thing.col(idx) = jt.translation();
}
static void
two(struct kinematic_hand_4f *hand)
two(struct KinematicHandCCDIK *hand)
{
XRT_TRACE_MARKER();
@ -105,7 +108,7 @@ moses_fast_simple_rotation(const Eigen::Vector3f &from_un, const Eigen::Vector3f
static void
do_it_for_bone(struct kinematic_hand_4f *hand, int finger_idx, int bone_idx, bool clamp_to_x_axis_rotation)
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];
@ -135,73 +138,8 @@ do_it_for_bone(struct kinematic_hand_4f *hand, int finger_idx, int bone_idx, boo
bone->bone_relation.linear() = bone->bone_relation.linear() * rot;
}
#if 1
static void
clamp_to_x_axis(struct kinematic_hand_4f *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;
int axis = 0;
Eigen::Vector3f axes[3] = {Eigen::Vector3f::UnitX(), Eigen::Vector3f::UnitY(), Eigen::Vector3f::UnitZ()};
// The input rotation will very likely rotate a vector pointed in the direction of axis we want to lock to...
// somewhere else. So, we find the new direction
Eigen::Vector3f axis_rotated_by_input = bone.bone_relation.linear() * axes[axis];
// And find a correction rotation that rotates our input rotation such that it doesn't affect vectors pointing
// in the direction of our axis anymore.
Eigen::Matrix3f correction_rot =
Eigen::Quaternionf().setFromTwoVectors(axis_rotated_by_input.normalized(), axes[axis]).matrix();
bone.bone_relation.linear() = correction_rot * bone.bone_relation.linear();
if (!clamp_angle) {
return;
}
Eigen::Vector3f axis_to_clamp_rotation = axes[(axis + 1) % 3];
Eigen::Vector3f new_ortho_direction = bone.bone_relation.linear() * axis_to_clamp_rotation;
float rotation_value = atan2(new_ortho_direction((axis + 2) % 3), new_ortho_direction((axis + 1) % 3));
if (rotation_value < max_angle && rotation_value > min_angle) {
return;
}
float positive_rotation_value, negative_rotation_value;
if (rotation_value < 0) {
positive_rotation_value = (M_PI * 2) - rotation_value;
negative_rotation_value = rotation_value;
} else {
negative_rotation_value = (-M_PI * 2) + rotation_value;
positive_rotation_value = rotation_value;
}
if ((positive_rotation_value - max_angle) > (min_angle - negative_rotation_value)) {
// Difference between max angle and positive value is higher, so we're closer to the minimum bound.
rotation_value = min_angle;
} else {
rotation_value = max_angle;
}
bone.bone_relation.linear() = Eigen::AngleAxisf(rotation_value, axes[axis]).toRotationMatrix();
}
#else
static void
clamp_to_x_axis(struct kinematic_hand_4f *hand,
clamp_to_x_axis(struct KinematicHandCCDIK *hand,
int finger_idx,
int bone_idx,
bool clamp_angle = false,
@ -242,7 +180,7 @@ clamp_to_x_axis(struct kinematic_hand_4f *hand,
// 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.
//! @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");
@ -262,7 +200,7 @@ clamp_to_x_axis(struct kinematic_hand_4f *hand,
//! @todo optimize: Move the asin into constexpr land
//! @optimize: Move the asin into constexpr land
// No, the sine of the joint limit
float rotation_value = asin(cross(0));
@ -282,12 +220,10 @@ clamp_to_x_axis(struct kinematic_hand_4f *hand,
// std::cout << n << "\n";
}
}
#endif
// Is this not just swing-twist about the Z axis? Dunnoooo... Find out later.
static void
clamp_proximals(struct kinematic_hand_4f *hand,
clamp_proximals(struct KinematicHandCCDIK *hand,
int finger_idx,
int bone_idx,
float max_swing_angle = 0,
@ -343,7 +279,7 @@ clamp_proximals(struct kinematic_hand_4f *hand,
if (our_z.z() > 0) {
//!@bug We need smarter joint limiting, limiting using tanangles is not acceptable as joints can rotate
//!@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;
}
@ -361,7 +297,7 @@ clamp_proximals(struct kinematic_hand_4f *hand,
static void
do_it_for_finger(struct kinematic_hand_4f *hand, int finger_idx)
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)));
@ -382,7 +318,7 @@ do_it_for_finger(struct kinematic_hand_4f *hand, int finger_idx)
}
static void
optimize(kinematic_hand_4f *hand)
optimize(KinematicHandCCDIK *hand)
{
for (int i = 0; i < 15; i++) {
two(hand);
@ -410,35 +346,35 @@ optimize(kinematic_hand_4f *hand)
static void
make_joint_at_matrix_left_hand(int idx, Eigen::Affine3f &pose, struct xrt_hand_joint_set *hand)
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)(
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();
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();
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)
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)(
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();
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();
@ -455,16 +391,16 @@ make_joint_at_matrix_right_hand(int idx, Eigen::Affine3f &pose, struct xrt_hand_
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();
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, int hand_idx)
make_joint_at_matrix(int idx, Eigen::Affine3f &pose, struct xrt_hand_joint_set &hand, bool is_right)
{
if (hand_idx == 0) {
if (!is_right) {
make_joint_at_matrix_left_hand(idx, pose, hand);
} else {
make_joint_at_matrix_right_hand(idx, pose, hand);
@ -473,19 +409,40 @@ make_joint_at_matrix(int idx, Eigen::Affine3f &pose, struct xrt_hand_joint_set *
// Exported:
void
optimize_new_frame(xrt_vec3 model_joints_3d[21],
kinematic_hand_4f *hand,
struct xrt_hand_joint_set *out_set,
int hand_idx)
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++) {
if (hand_idx == 0) {
hand->t_jts[i] = model_joints_3d[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 = -model_joints_3d[i].x;
hand->t_jts[i].y = model_joints_3d[i].y;
hand->t_jts[i].z = model_joints_3d[i].z;
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;
@ -498,7 +455,7 @@ optimize_new_frame(xrt_vec3 model_joints_3d[21],
// Convert it to xrt_hand_joint_set!
make_joint_at_matrix(XRT_HAND_JOINT_WRIST, hand->wrist_relation, out_set, hand_idx);
make_joint_at_matrix(XRT_HAND_JOINT_WRIST, hand->wrist_relation, out_set, hand->is_right);
Eigen::Affine3f palm_relation;
@ -508,7 +465,7 @@ optimize_new_frame(xrt_vec3 model_joints_3d[21],
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_idx);
make_joint_at_matrix(XRT_HAND_JOINT_PALM, palm_relation, out_set, hand->is_right);
int start = XRT_HAND_JOINT_THUMB_METACARPAL;
@ -519,25 +476,34 @@ optimize_new_frame(xrt_vec3 model_joints_3d[21],
if (!(finger_idx == 0 && bone_idx == 0)) {
make_joint_at_matrix(start++, hand->fingers[finger_idx].bones[bone_idx].world_pose,
out_set, hand_idx);
out_set, hand->is_right);
}
}
}
out_set->is_active = true;
out_set.is_active = true;
}
void
alloc_kinematic_hand(kinematic_hand_4f **out_kinematic_hand)
alloc_kinematic_hand(xrt_pose left_in_right, bool is_right, KinematicHandCCDIK **out_kinematic_hand)
{
kinematic_hand_4f *h = new kinematic_hand_4f;
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(kinematic_hand_4f **kinematic_hand)
free_kinematic_hand(KinematicHandCCDIK **kinematic_hand)
{
delete *kinematic_hand;
}
} // namespace xrt::tracking::hand::mercury::kine
} // namespace xrt::tracking::hand::mercury::ccdik

10
src/xrt/tracking/hand/mercury/kine/kinematic_tiny_math.hpp → src/xrt/tracking/hand/mercury/kine_ccdik/ccdik_tiny_math.hpp
View file

@ -8,9 +8,9 @@
*/
#pragma once
#include "kinematic_defines.hpp"
#include "ccdik_defines.hpp"
namespace xrt::tracking::hand::mercury::kine {
namespace xrt::tracking::hand::mercury::ccdik {
// Waggle-curl-twist.
static inline void
wct_to_quat(wct_t wct, struct xrt_quat *out)
@ -28,7 +28,9 @@ wct_to_quat(wct_t wct, struct xrt_quat *out)
xrt_quat just_twist;
math_quat_from_angle_vector(wct.twist, &twist_axis, &just_twist);
//! @todo: optimize This should be a matrix multiplication...
//! @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);
@ -52,4 +54,4 @@ clamp_to_r(float *in, float c, float r)
{
clamp(in, c - r, c + r);
}
} // namespace xrt::tracking::hand::mercury::kine
} // namespace xrt::tracking::hand::mercury::ccdik

89
src/xrt/tracking/hand/mercury/kine_ccdik/lineline.hpp Normal file
View file

@ -0,0 +1,89 @@
// 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);
}

67
src/xrt/tracking/hand/mercury/kine_common.hpp Normal file
View file

@ -0,0 +1,67 @@
// Copyright 2022, Collabora, Ltd.
// SPDX-License-Identifier: BSL-1.0
/*!
* @file
* @brief Random common stuff for Mercury kinematic optimizers
* @author Moses Turner <moses@collabora.com>
* @ingroup tracking
*/
#pragma once
#include "xrt/xrt_defines.h"
namespace xrt::tracking::hand::mercury {
// Changing this to double should work but you might need to fix some things.
// Float is faster, and nothing (should) be too big or too small to require double.
// Different from `Scalar` in lm_* templates - those can be `Ceres::Jet`s too.
typedef float HandScalar;
// Inputs to kinematic optimizers
//!@todo Ask Ryan if adding `= {}` only does something if we do one_frame_one_view bla = {}.
struct one_frame_one_view
{
bool active = true;
xrt_vec3 rays[21] = {};
HandScalar confidences[21] = {};
};
struct one_frame_input
{
one_frame_one_view views[2] = {};
};
namespace Joint21 {
enum Joint21
{
WRIST = 0,
THMB_MCP = 1,
THMB_PXM = 2,
THMB_DST = 3,
THMB_TIP = 4,
INDX_PXM = 5,
INDX_INT = 6,
INDX_DST = 7,
INDX_TIP = 8,
MIDL_PXM = 9,
MIDL_INT = 10,
MIDL_DST = 11,
MIDL_TIP = 12,
RING_PXM = 13,
RING_INT = 14,
RING_DST = 15,
RING_TIP = 16,
LITL_PXM = 17,
LITL_INT = 18,
LITL_DST = 19,
LITL_TIP = 20
};
}
} // namespace xrt::tracking::hand::mercury

30
src/xrt/tracking/hand/mercury/kine_lm/CMakeLists.txt Normal file
View file

@ -0,0 +1,30 @@
# Copyright 2022, Collabora, Ltd.
# SPDX-License-Identifier: BSL-1.0
add_library(t_ht_mercury_kine_lm STATIC lm_interface.hpp lm_main.cpp)
target_link_libraries(
t_ht_mercury_kine_lm
PRIVATE
aux_math
aux_tracking
aux_os
aux_util
xrt-external-tinyceres
)
target_include_directories(t_ht_mercury_kine_lm SYSTEM PRIVATE ${EIGEN3_INCLUDE_DIR})
if(${CMAKE_CXX_COMPILER_ID} STREQUAL "Clang")
target_compile_options(t_ht_mercury_kine_lm PRIVATE -ftemplate-backtrace-limit=20)
endif()
# Below is entirely just so that tests can find us.
add_library(
t_ht_mercury_kine_lm_includes INTERFACE
)
target_include_directories(
t_ht_mercury_kine_lm_includes INTERFACE ${CMAKE_CURRENT_SOURCE_DIR} ${CMAKE_CURRENT_BINARY_DIR}
)

318
src/xrt/tracking/hand/mercury/kine_lm/lm_defines.hpp Normal file
View file

@ -0,0 +1,318 @@
// Copyright 2022, Collabora, Ltd.
// SPDX-License-Identifier: BSL-1.0
/*!
* @file
* @brief Defines for Levenberg-Marquardt kinematic optimizer
* @author Moses Turner <moses@collabora.com>
* @ingroup tracking
*/
#pragma once
// #include <Eigen/Core>
// #include <Eigen/Geometry>
#include "math/m_mathinclude.h"
#include "../kine_common.hpp"
#include <type_traits>
namespace xrt::tracking::hand::mercury::lm {
#define LM_TRACE(lmh, ...) U_LOG_IFL_T(lmh.log_level, __VA_ARGS__)
#define LM_DEBUG(lmh, ...) U_LOG_IFL_D(lmh.log_level, __VA_ARGS__)
#define LM_INFO(lmh, ...) U_LOG_IFL_I(lmh.log_level, __VA_ARGS__)
#define LM_WARN(lmh, ...) U_LOG_IFL_W(lmh.log_level, __VA_ARGS__)
#define LM_ERROR(lmh, ...) U_LOG_IFL_E(lmh.log_level, __VA_ARGS__)
// Inlines.
template <typename T>
inline T
rad(T degrees)
{
return degrees * T(M_PI / 180.f);
}
// Number of joints that our ML models output.
static constexpr size_t kNumNNJoints = 21;
static constexpr size_t kNumFingers = 5;
// This is a lie for the thumb; we usually do the hidden metacarpal trick there
static constexpr size_t kNumJointsInFinger = 5;
static constexpr size_t kNumOrientationsInFinger = 4;
// These defines look silly, but they are _extremely_ useful for doing work on this optimizer. Please don't remove them.
#define USE_HAND_SIZE
#define USE_HAND_TRANSLATION
#define USE_HAND_ORIENTATION
#define USE_EVERYTHING_ELSE
// Not tested/tuned well enough; might make tracking slow.
#undef USE_HAND_PLAUSIBILITY
static constexpr size_t kMetacarpalBoneDim = 3;
static constexpr size_t kProximalBoneDim = 2;
static constexpr size_t kFingerDim = kProximalBoneDim + kMetacarpalBoneDim + 2;
static constexpr size_t kThumbDim = kMetacarpalBoneDim + 2;
static constexpr size_t kHandSizeDim = 1;
static constexpr size_t kHandTranslationDim = 3;
static constexpr size_t kHandOrientationDim = 3;
static constexpr size_t kHRTC_HandSize = 1;
static constexpr size_t kHRTC_RootBoneTranslation = 3;
static constexpr size_t kHRTC_RootBoneOrientation = 3; // Direct difference between the two angle-axis rotations. This
// works well enough because the rotation should be small.
static constexpr size_t kHRTC_ThumbMCPSwingTwist = 3;
static constexpr size_t kHRTC_ThumbCurls = 2;
static constexpr size_t kHRTC_ProximalSimilarity = 2;
static constexpr size_t kHRTC_FingerMCPSwingTwist = 3;
static constexpr size_t kHRTC_FingerPXMSwing = 2;
static constexpr size_t kHRTC_FingerCurls = 2;
static constexpr size_t kHRTC_CurlSimilarity = 1;
static constexpr size_t kHandResidualOneSideSize = 21 * 2;
static constexpr size_t kHandResidualTemporalConsistencyOneFingerSize = //
kHRTC_FingerMCPSwingTwist + //
kHRTC_FingerPXMSwing + //
kHRTC_FingerCurls + //
#ifdef USE_HAND_PLAUSIBILITY //
kHRTC_CurlSimilarity + //
#endif //
0;
static constexpr size_t kHandResidualTemporalConsistencySize = //
kHRTC_RootBoneTranslation + //
kHRTC_RootBoneOrientation + //
kHRTC_ThumbMCPSwingTwist + //
kHRTC_ThumbCurls + //
#ifdef USE_HAND_PLAUSIBILITY //
kHRTC_ProximalSimilarity + //
#endif //
(kHandResidualTemporalConsistencyOneFingerSize * 4) + //
0;
// Factors to multiply different values by to get a smooth hand trajectory without introducing too much latency
// 1.0 is good, a little jittery.
// Anything above 3.0 generally breaks.
static constexpr HandScalar kStabilityRoot = 1.0;
static constexpr HandScalar kStabilityCurlRoot = kStabilityRoot * 0.03f;
static constexpr HandScalar kStabilityOtherRoot = kStabilityRoot * 0.03f;
static constexpr HandScalar kStabilityThumbMCPSwing = kStabilityCurlRoot * 1.5f;
static constexpr HandScalar kStabilityThumbMCPTwist = kStabilityCurlRoot * 1.5f;
static constexpr HandScalar kStabilityFingerMCPSwing = kStabilityCurlRoot * 3.0f;
static constexpr HandScalar kStabilityFingerMCPTwist = kStabilityCurlRoot * 3.0f;
static constexpr HandScalar kStabilityFingerPXMSwingX = kStabilityCurlRoot * 1.0f;
static constexpr HandScalar kStabilityFingerPXMSwingY = kStabilityCurlRoot * 1.6f;
static constexpr HandScalar kStabilityRootPosition = kStabilityOtherRoot * 30;
static constexpr HandScalar kStabilityHandSize = kStabilityOtherRoot * 1000;
static constexpr HandScalar kStabilityHandOrientation = kStabilityOtherRoot * 3;
static constexpr HandScalar kPlausibilityRoot = 1.0;
static constexpr HandScalar kPlausibilityProximalSimilarity = 0.05f * kPlausibilityRoot;
static constexpr HandScalar kPlausibilityCurlSimilarityHard = 0.10f * kPlausibilityRoot;
static constexpr HandScalar kPlausibilityCurlSimilaritySoft = 0.05f * kPlausibilityRoot;
constexpr size_t
calc_input_size(bool optimize_hand_size)
{
size_t out = 0;
#ifdef USE_HAND_TRANSLATION
out += kHandTranslationDim;
#endif
#ifdef USE_HAND_ORIENTATION
out += kHandOrientationDim;
#endif
#ifdef USE_EVERYTHING_ELSE
out += kThumbDim;
out += (kFingerDim * 4);
#endif
#ifdef USE_HAND_SIZE
if (optimize_hand_size) {
out += kHandSizeDim;
}
#endif
return out;
}
constexpr size_t
calc_residual_size(bool stability, bool optimize_hand_size, int num_views)
{
size_t out = 0;
for (int i = 0; i < num_views; i++) {
out += kHandResidualOneSideSize;
}
if (stability) {
out += kHandResidualTemporalConsistencySize;
}
if (optimize_hand_size) {
out += kHRTC_HandSize;
}
return out;
}
// Some templatable spatial types.
// Heavily inspired by Eigen - one can definitely use Eigen instead, but here I'd rather have more control
template <typename Scalar> struct Quat
{
Scalar x;
Scalar y;
Scalar z;
Scalar w;
/// Default constructor - DOES NOT INITIALIZE VALUES
constexpr Quat() {}
/// Copy constructor
constexpr Quat(Quat const &) noexcept(std::is_nothrow_copy_constructible_v<Scalar>) = default;
/// Move constructor
Quat(Quat &&) noexcept(std::is_nothrow_move_constructible_v<Scalar>) = default;
/// Copy assignment
Quat &
operator=(Quat const &) = default;
/// Move assignment
Quat &
operator=(Quat &&) noexcept = default;
/// Construct from x, y, z, w scalars
template <typename Other>
constexpr Quat(Other x, Other y, Other z, Other w) noexcept // NOLINT(bugprone-easily-swappable-parameters)
: x{Scalar(x)}, y{Scalar(y)}, z{Scalar(z)}, w{Scalar(w)}
{}
/// So that we can copy a regular Vec2 into the real part of a Jet Vec2
template <typename Other> Quat(Quat<Other> const &other) : Quat(other.x, other.y, other.z, other.w) {}
static Quat
Identity()
{
return Quat(0.f, 0.f, 0.f, 1.f);
}
};
template <typename Scalar> struct Vec3
{
// Note that these are not initialized, for performance reasons.
// If you want them initialized, use Zero() or something else
Scalar x;
Scalar y;
Scalar z;
/// Default constructor - DOES NOT INITIALIZE VALUES
constexpr Vec3() {}
/// Copy constructor
constexpr Vec3(Vec3 const &other) noexcept(std::is_nothrow_copy_constructible_v<Scalar>) = default;
/// Move constructor
Vec3(Vec3 &&) noexcept(std::is_nothrow_move_constructible_v<Scalar>) = default;
/// Copy assignment
Vec3 &
operator=(Vec3 const &) = default;
/// Move assignment
Vec3 &
operator=(Vec3 &&) noexcept = default;
template <typename Other>
constexpr Vec3(Other x, Other y, Other z) noexcept // NOLINT(bugprone-easily-swappable-parameters)
: x{Scalar(x)}, y{Scalar(y)}, z{Scalar(z)}
{}
template <typename Other> Vec3(Vec3<Other> const &other) : Vec3(other.x, other.y, other.z) {}
static Vec3
Zero()
{
return Vec3(0.f, 0.f, 0.f);
}
};
template <typename Scalar> struct Vec2
{
Scalar x;
Scalar y;
/// Default constructor - DOES NOT INITIALIZE VALUES
constexpr Vec2() noexcept {}
/// Copy constructor
constexpr Vec2(Vec2 const &) noexcept(std::is_nothrow_copy_constructible_v<Scalar>) = default;
/// Move constructor
constexpr Vec2(Vec2 &&) noexcept(std::is_nothrow_move_constructible_v<Scalar>) = default;
/// Copy assignment
Vec2 &
operator=(Vec2 const &) = default;
/// Move assignment
Vec2 &
operator=(Vec2 &&) noexcept = default;
/// So that we can copy a regular Vec2 into the real part of a Jet Vec2
template <typename Other>
Vec2(Other x, Other y) // NOLINT(bugprone-easily-swappable-parameters)
noexcept(std::is_nothrow_constructible_v<Scalar, Other>)
: x{Scalar(x)}, y{Scalar(y)}
{}
template <typename Other>
Vec2(Vec2<Other> const &other) noexcept(std::is_nothrow_constructible_v<Scalar, Other>) : Vec2(other.x, other.y)
{}
static constexpr Vec2
Zero()
{
return Vec2(0.f, 0.f);
}
};
template <typename T> struct ResidualHelper
{
T *out_residual;
size_t out_residual_idx = 0;
ResidualHelper(T *residual) : out_residual(residual)
{
out_residual_idx = 0;
}
void
AddValue(T const &value)
{
this->out_residual[out_residual_idx++] = value;
}
};
} // namespace xrt::tracking::hand::mercury::lm

62
src/xrt/tracking/hand/mercury/kine_lm/lm_interface.hpp Normal file
View file

@ -0,0 +1,62 @@
// Copyright 2022, Collabora, Ltd.
// SPDX-License-Identifier: BSL-1.0
/*!
* @file
* @brief Interface for Levenberg-Marquardt kinematic optimizer
* @author Moses Turner <moses@collabora.com>
* @ingroup tracking
*/
#pragma once
#include "xrt/xrt_defines.h"
#include "util/u_logging.h"
// #include "lm_defines.hpp"
#include "../kine_common.hpp"
namespace xrt::tracking::hand::mercury::lm {
// Yes, this is a weird in-between-C-and-C++ API. Fight me, I like it this way.
// Opaque struct.
struct KinematicHandLM;
// Constructor
void
optimizer_create(xrt_pose left_in_right,
bool is_right,
u_logging_level log_level,
KinematicHandLM **out_kinematic_hand);
/*!
* The main tracking code calls this function with some 2D(ish) camera observations of the hand, and this function
* calculates a good 3D hand pose and writes it to out_viz_hand.
*
* @param observation The observation of the hand joints. Warning, this function will mutate the observation
* unpredictably. Keep a copy of it if you need it after.
* @param hand_was_untracked_last_frame: If the hand was untracked last frame (it was out of view, obscured, ML models
* failed, etc.) - if it was, we don't want to enforce temporal consistency because we have no good previous hand state
* with which to do that.
* @param optimize_hand_size: Whether or not it's allowed to tweak the hand size - when we're calibrating the user's
* hand size, we want to do that; afterwards we don't want to waste the compute.
* @param target_hand_size: The hand size we want it to get close to
* @param hand_size_err_mul: A multiplier to help determine how close it has to get to that hand size
* @param[out] out_hand: The xrt_hand_joint_set to output its result to
* @param[out] out_hand_size: The hand size it ended up at
* @param[out] out_reprojection_error: The reprojection error it ended up at
*/
void
optimizer_run(KinematicHandLM *hand,
one_frame_input &observation,
bool hand_was_untracked_last_frame,
bool optimize_hand_size,
float target_hand_size,
float hand_size_err_mul,
xrt_hand_joint_set &out_hand,
float &out_hand_size,
float &out_reprojection_error);
// Destructor
void
optimizer_destroy(KinematicHandLM **hand);
} // namespace xrt::tracking::hand::mercury::lm

992
src/xrt/tracking/hand/mercury/kine_lm/lm_main.cpp Normal file
View file

@ -0,0 +1,992 @@
// Copyright 2022, Collabora, Ltd.
// SPDX-License-Identifier: BSL-1.0
/*!
* @file
* @brief Levenberg-Marquardt kinematic optimizer
* @author Moses Turner <moses@collabora.com>
* @author Charlton Rodda <charlton.rodda@collabora.com>
* @ingroup tracking
*/
#include "math/m_api.h"
#include "math/m_vec3.h"
#include "os/os_time.h"
#include "util/u_logging.h"
#include "util/u_misc.h"
#include "util/u_trace_marker.h"
#include "tinyceres/tiny_solver.hpp"
#include "tinyceres/tiny_solver_autodiff_function.hpp"
#include "lm_rotations.inl"
#include <iostream>
#include <cmath>
#include <random>
#include "lm_interface.hpp"
#include "lm_optimizer_params_packer.inl"
#include "lm_defines.hpp"
/*
Some notes:
Everything templated with <typename T> is basically just a scalar template, usually taking float or ceres::Jet<float, N>
*/
namespace xrt::tracking::hand::mercury::lm {
template <typename T> struct StereographicObservation
{
// T obs[kNumNNJoints][2];
Vec2<T> obs[kNumNNJoints];
};
struct KinematicHandLM
{
bool first_frame = true;
bool use_stability = false;
bool optimize_hand_size = true;
bool is_right = false;
int num_observation_views = 0;
one_frame_input *observation;
HandScalar target_hand_size;
HandScalar hand_size_err_mul;
u_logging_level log_level;
StereographicObservation<HandScalar> sgo[2];
Quat<HandScalar> last_frame_pre_rotation;
OptimizerHand<HandScalar> last_frame;
// The pose that will take you from the right camera's space to the left camera's space.
xrt_pose left_in_right;
// The translation part of the same pose, just easier for Ceres to consume
Vec3<HandScalar> left_in_right_translation;
// The orientation part of the same pose, just easier for Ceres to consume
Quat<HandScalar> left_in_right_orientation;
Eigen::Matrix<HandScalar, calc_input_size(true), 1> TinyOptimizerInput;
};
template <typename T> struct Translations55
{
Vec3<T> t[kNumFingers][kNumJointsInFinger];
};
template <typename T> struct Orientations54
{
Quat<T> q[kNumFingers][kNumJointsInFinger];
};
template <bool optimize_hand_size> struct CostFunctor
{
KinematicHandLM &parent;
size_t num_residuals_;
template <typename T>
bool
operator()(const T *const x, T *residual) const;
CostFunctor(KinematicHandLM &in_last_hand, size_t const &num_residuals)
: parent(in_last_hand), num_residuals_(num_residuals)
{}
size_t
NumResiduals() const
{
return num_residuals_;
}
};
template <typename T>
static inline void
eval_hand_set_rel_translations(const OptimizerHand<T> &opt, Translations55<T> &rel_translations)
{
// Basically, we're walking up rel_translations, writing strictly sequentially. Hopefully this is fast.
// Thumb metacarpal translation.
rel_translations.t[0][0] = {(T)0.33097, T(-0.1), (T)-0.25968};
// Comes after the invisible joint.
rel_translations.t[0][1] = {T(0), T(0), T(0)};
// prox, distal, tip
rel_translations.t[0][2] = {T(0), T(0), T(-0.389626)};
rel_translations.t[0][3] = {T(0), T(0), T(-0.311176)};
rel_translations.t[0][4] = {T(0), T(0), (T)-0.232195};
// What's the best place to put this? Here works, but is there somewhere we could put it where it gets accessed
// faster?
T finger_joint_lengths[4][4] = {
{
T(-0.66),
T(-0.365719),
T(-0.231581),
T(-0.201790),
},
{
T(-0.645),
T(-0.404486),
T(-0.247749),
T(-0.210121),
},
{
T(-0.58),
T(-0.365639),
T(-0.225666),
T(-0.187089),
},
{
T(-0.52),
T(-0.278197),
T(-0.176178),
T(-0.157566),
},
};
// Index metacarpal
rel_translations.t[1][0] = {T(0.16926), T(0), T(-0.34437)};
// Middle
rel_translations.t[2][0] = {T(0.034639), T(0.01), T(-0.35573)};
// Ring
rel_translations.t[3][0] = {T(-0.063625), T(0.005), T(-0.34164)};
// Little
rel_translations.t[4][0] = {T(-0.1509), T(-0.005), T(-0.30373)};
// Index to little finger
for (int finger = 0; finger < 4; finger++) {
for (int i = 0; i < 4; i++) {
int bone = i + 1;
rel_translations.t[finger + 1][bone] = {T(0), T(0), T(finger_joint_lengths[finger][i])};
}
}
// This is done in UnitQuaternionRotateAndScale now.
// for (int finger = 0; finger < 5; finger++) {
// for (int bone = 0; bone < 5; bone++) {
// rel_translations[finger][bone][0] *= opt.hand_size;
// rel_translations[finger][bone][1] *= opt.hand_size;
// rel_translations[finger][bone][2] *= opt.hand_size;
// }
// }
}
template <typename T>
inline void
eval_hand_set_rel_orientations(const OptimizerHand<T> &opt, Orientations54<T> &rel_orientations)
{
// Thumb MCP hidden orientation
#if 0
Vec2<T> mcp_root_swing;
mcp_root_swing.x = rad<T>((T)(-10));
mcp_root_swing.y = rad<T>((T)(-40));
T mcp_root_twist = rad<T>((T)(-80));
SwingTwistToQuaternion(mcp_root_swing, mcp_root_twist, rel_orientations.q[0][0]);
std::cout << "\n\n\n\nHIDDEN ORIENTATION\n";
std::cout << std::setprecision(100);
std::cout << rel_orientations.q[0][0].w << std::endl;
std::cout << rel_orientations.q[0][0].x << std::endl;
std::cout << rel_orientations.q[0][0].y << std::endl;
std::cout << rel_orientations.q[0][0].z << std::endl;
#else
// This should be exactly equivalent to the above
rel_orientations.q[0][0].w = T(0.716990172863006591796875);
rel_orientations.q[0][0].x = T(0.1541481912136077880859375);
rel_orientations.q[0][0].y = T(-0.31655871868133544921875);
rel_orientations.q[0][0].z = T(-0.6016261577606201171875);
#endif
// Thumb MCP orientation
SwingTwistToQuaternion(opt.thumb.metacarpal.swing, //
opt.thumb.metacarpal.twist, //
rel_orientations.q[0][1]);
// Thumb curls
CurlToQuaternion(opt.thumb.rots[0], rel_orientations.q[0][2]);
CurlToQuaternion(opt.thumb.rots[1], rel_orientations.q[0][3]);
// Finger orientations
for (int i = 0; i < 4; i++) {
SwingTwistToQuaternion(opt.finger[i].metacarpal.swing, //
opt.finger[i].metacarpal.twist, //
rel_orientations.q[i + 1][0]);
SwingToQuaternion(opt.finger[i].proximal_swing, //
rel_orientations.q[i + 1][1]);
CurlToQuaternion(opt.finger[i].rots[0], rel_orientations.q[i + 1][2]);
CurlToQuaternion(opt.finger[i].rots[1], rel_orientations.q[i + 1][3]);
}
}
template <typename T>
void
eval_hand_with_orientation(const OptimizerHand<T> &opt,
bool is_right,
Translations55<T> &translations_absolute,
Orientations54<T> &orientations_absolute)
{
XRT_TRACE_MARKER();
Translations55<T> rel_translations; //[kNumFingers][kNumJointsInFinger];
Orientations54<T> rel_orientations; //[kNumFingers][kNumOrientationsInFinger];
eval_hand_set_rel_orientations(opt, rel_orientations);
eval_hand_set_rel_translations(opt, rel_translations);
Quat<T> orientation_root;
Quat<T> post_orientation_quat;
AngleAxisToQuaternion(opt.wrist_post_orientation_aax, post_orientation_quat);
QuaternionProduct(opt.wrist_pre_orientation_quat, post_orientation_quat, orientation_root);
// Get each joint's tracking-relative orientation by rotating its parent-relative orientation by the
// tracking-relative orientation of its parent.
for (size_t finger = 0; finger < kNumFingers; finger++) {
Quat<T> *last_orientation = &orientation_root;
for (size_t bone = 0; bone < kNumOrientationsInFinger; bone++) {
Quat<T> &out_orientation = orientations_absolute.q[finger][bone];
Quat<T> &rel_orientation = rel_orientations.q[finger][bone];
QuaternionProduct(*last_orientation, rel_orientation, out_orientation);
last_orientation = &out_orientation;
}
}
// Get each joint's tracking-relative position by rotating its parent-relative translation by the
// tracking-relative orientation of its parent, then adding that to its parent's tracking-relative position.
for (size_t finger = 0; finger < kNumFingers; finger++) {
const Vec3<T> *last_translation = &opt.wrist_location;
const Quat<T> *last_orientation = &orientation_root;
for (size_t bone = 0; bone < kNumJointsInFinger; bone++) {
Vec3<T> &out_translation = translations_absolute.t[finger][bone];
Vec3<T> &rel_translation = rel_translations.t[finger][bone];
UnitQuaternionRotateAndScalePoint(*last_orientation, rel_translation, opt.hand_size,
out_translation);
// If this is a right hand, mirror it.
if (is_right) {
out_translation.x *= -1;
}
out_translation.x += last_translation->x;
out_translation.y += last_translation->y;
out_translation.z += last_translation->z;
// Next iteration, the orientation to rotate by should be the tracking-relative orientation of
// this joint.
// If bone < 4 so we don't go over the end of orientations_absolute. I hope this gets optimized
// out anyway.
if (bone < 4) {
last_orientation = &orientations_absolute.q[finger][bone];
// Ditto for translation
last_translation = &out_translation;
}
}
}
}
template <typename T>
void
computeResidualStability_Finger(const OptimizerFinger<T> &finger,
const OptimizerFinger<HandScalar> &finger_last,
ResidualHelper<T> &helper)
{
helper.AddValue((finger.metacarpal.swing.x - finger_last.metacarpal.swing.x) * kStabilityFingerMCPSwing);
helper.AddValue((finger.metacarpal.swing.y - finger_last.metacarpal.swing.y) * kStabilityFingerMCPSwing);
helper.AddValue((finger.metacarpal.twist - finger_last.metacarpal.twist) * kStabilityFingerMCPTwist);
helper.AddValue((finger.proximal_swing.x - finger_last.proximal_swing.x) * kStabilityFingerPXMSwingX);
helper.AddValue((finger.proximal_swing.y - finger_last.proximal_swing.y) * kStabilityFingerPXMSwingY);
helper.AddValue((finger.rots[0] - finger_last.rots[0]) * kStabilityCurlRoot);
helper.AddValue((finger.rots[1] - finger_last.rots[1]) * kStabilityCurlRoot);
#ifdef USE_HAND_PLAUSIBILITY
if (finger.rots[0] < finger.rots[1]) {
helper.AddValue((finger.rots[0] - finger.rots[1]) * kPlausibilityCurlSimilarityHard);
} else {
helper.AddValue((finger.rots[0] - finger.rots[1]) * kPlausibilityCurlSimilaritySoft);
}
#endif
}
template <bool optimize_hand_size, typename T>
void
computeResidualStability(const OptimizerHand<T> &hand,
const OptimizerHand<HandScalar> &last_hand,
KinematicHandLM &state,
ResidualHelper<T> &helper)
{
if constexpr (optimize_hand_size) {
helper.AddValue( //
(hand.hand_size - state.target_hand_size) * (T)(kStabilityHandSize * state.hand_size_err_mul));
}
if (state.first_frame) {
return;
}
helper.AddValue((last_hand.wrist_location.x - hand.wrist_location.x) * kStabilityRootPosition);
helper.AddValue((last_hand.wrist_location.y - hand.wrist_location.y) * kStabilityRootPosition);
helper.AddValue((last_hand.wrist_location.z - hand.wrist_location.z) * kStabilityRootPosition);
helper.AddValue((hand.wrist_post_orientation_aax.x) * (T)(kStabilityHandOrientation));
helper.AddValue((hand.wrist_post_orientation_aax.y) * (T)(kStabilityHandOrientation));
helper.AddValue((hand.wrist_post_orientation_aax.z) * (T)(kStabilityHandOrientation));
helper.AddValue((hand.thumb.metacarpal.swing.x - last_hand.thumb.metacarpal.swing.x) * kStabilityThumbMCPSwing);
helper.AddValue((hand.thumb.metacarpal.swing.y - last_hand.thumb.metacarpal.swing.y) * kStabilityThumbMCPSwing);
helper.AddValue((hand.thumb.metacarpal.twist - last_hand.thumb.metacarpal.twist) * kStabilityThumbMCPTwist);
helper.AddValue((hand.thumb.rots[0] - last_hand.thumb.rots[0]) * kStabilityCurlRoot);
helper.AddValue((hand.thumb.rots[1] - last_hand.thumb.rots[1]) * kStabilityCurlRoot);
#ifdef USE_HAND_PLAUSIBILITY
helper.AddValue((hand.finger[1].proximal_swing.x - hand.finger[2].proximal_swing.x) *
kPlausibilityProximalSimilarity);
helper.AddValue((hand.finger[2].proximal_swing.x - hand.finger[3].proximal_swing.x) *
kPlausibilityProximalSimilarity);
#endif
for (int finger_idx = 0; finger_idx < 4; finger_idx++) {
const OptimizerFinger<HandScalar> &finger_last = last_hand.finger[finger_idx];
const OptimizerFinger<T> &finger = hand.finger[finger_idx];
computeResidualStability_Finger(finger, finger_last, helper);
}
}
template <typename T>
inline void
normalize_vector_inplace(Vec3<T> &vector)
{
T len = (T)(0);
len += vector.x * vector.x;
len += vector.y * vector.y;
len += vector.z * vector.z;
len = sqrt(len);
// This is *a* solution ;)
//!@todo any good template ways to get epsilon for float,double,jet?
if (len <= FLT_EPSILON) {
vector.z = T(-1);
return;
}
vector.x /= len;
vector.y /= len;
vector.z /= len;
}
// in size: 3, out size: 2
template <typename T>
static inline void
unit_vector_stereographic_projection(const Vec3<T> &in, Vec2<T> &out)
{
out.x = in.x / ((T)1 - in.z);
out.y = in.y / ((T)1 - in.z);
}
template <typename T>
static inline void
unit_xrt_vec3_stereographic_projection(const xrt_vec3 in, Vec2<T> &out)
{
Vec3<T> vec;
vec.x = (T)(in.x);
vec.y = (T)(in.y);
vec.z = (T)(in.z);
normalize_vector_inplace(vec);
unit_vector_stereographic_projection(vec, out);
}
template <typename T>
static void
diff(const Vec3<T> &model_joint_pos,
const Vec3<T> &move_joint_translation,
const Quat<T> &move_joint_orientation,
const StereographicObservation<HandScalar> &observation,
const HandScalar *confidences,
const HandScalar amount_we_care,
int &hand_joint_idx,
ResidualHelper<T> &helper)
{
Vec3<T> model_joint_dir_rel_camera;
UnitQuaternionRotatePoint(move_joint_orientation, model_joint_pos, model_joint_dir_rel_camera);
model_joint_dir_rel_camera.x = model_joint_dir_rel_camera.x + move_joint_translation.x;
model_joint_dir_rel_camera.y = model_joint_dir_rel_camera.y + move_joint_translation.y;
model_joint_dir_rel_camera.z = model_joint_dir_rel_camera.z + move_joint_translation.z;
normalize_vector_inplace(model_joint_dir_rel_camera);
Vec2<T> stereographic_model_dir;
unit_vector_stereographic_projection(model_joint_dir_rel_camera, stereographic_model_dir);
const HandScalar confidence = confidences[hand_joint_idx] * amount_we_care;
const Vec2<T> &observed_ray_sg = observation.obs[hand_joint_idx];
helper.AddValue((stereographic_model_dir.x - (T)(observed_ray_sg.x)) * confidence);
helper.AddValue((stereographic_model_dir.y - (T)(observed_ray_sg.y)) * confidence);
hand_joint_idx++;
}
template <typename T>
void
CostFunctor_PositionsPart(OptimizerHand<T> &hand, KinematicHandLM &state, ResidualHelper<T> &helper)
{
Translations55<T> translations_absolute;
Orientations54<T> orientations_absolute;
HandScalar we_care_joint[] = {1.3, 0.9, 0.9, 1.3};
HandScalar we_care_finger[] = {1.0, 1.0, 0.8, 0.8, 0.8};
eval_hand_with_orientation(hand, state.is_right, translations_absolute, orientations_absolute);
for (int view = 0; view < 2; view++) {
if (!state.observation->views[view].active) {
continue;
}
Vec3<T> move_direction;
Quat<T> move_orientation;
if (view == 0) {
move_direction = Vec3<T>::Zero();
move_orientation = Quat<T>::Identity();
} else {
move_direction.x = T(state.left_in_right_translation.x);
move_direction.y = T(state.left_in_right_translation.y);
move_direction.z = T(state.left_in_right_translation.z);
move_orientation.w = T(state.left_in_right_orientation.w);
move_orientation.x = T(state.left_in_right_orientation.x);
move_orientation.y = T(state.left_in_right_orientation.y);
move_orientation.z = T(state.left_in_right_orientation.z);
}
int joint_acc_idx = 0;
HandScalar *confidences = state.observation->views[view].confidences;
diff<T>(hand.wrist_location, move_direction, move_orientation, state.sgo[view], confidences, 1.5,
joint_acc_idx, helper);
for (int finger_idx = 0; finger_idx < 5; finger_idx++) {
for (int joint_idx = 0; joint_idx < 4; joint_idx++) {
diff<T>(translations_absolute.t[finger_idx][joint_idx + 1], move_direction,
move_orientation, state.sgo[view], confidences,
we_care_finger[finger_idx] * we_care_joint[joint_idx], joint_acc_idx, helper);
}
}
}
}
// template <typename T>
// void CostFunctor_HandSizePart(OptimizerHand<T> &hand, KinematicHandLM &state, T *residual, size_t &out_residual_idx)
// {
// }
template <bool optimize_hand_size>
template <typename T>
bool
CostFunctor<optimize_hand_size>::operator()(const T *const x, T *residual) const
{
struct KinematicHandLM &state = this->parent;
OptimizerHand<T> hand = {};
// ??? should I do the below? probably.
Quat<T> tmp = this->parent.last_frame_pre_rotation;
OptimizerHandInit<T>(hand, tmp);
OptimizerHandUnpackFromVector(x, state.optimize_hand_size, T(state.target_hand_size), hand);
XRT_MAYBE_UNUSED size_t residual_size =
calc_residual_size(state.use_stability, optimize_hand_size, state.num_observation_views);
// When you're hacking, you want to set the residuals to always-0 so that any of them you forget to touch keep their 0
// gradient.
// But then later this just becomes a waste.
#if 0
for (size_t i = 0; i < residual_size; i++) {
residual[i] = (T)(0);
}
#endif
ResidualHelper<T> helper(residual);
CostFunctor_PositionsPart(hand, state, helper);
computeResidualStability<optimize_hand_size, T>(hand, state.last_frame, state, helper);
// Bounds checking - we should have written exactly to the end.
// U_LOG_E("%zu %zu", helper.out_residual_idx, residual_size);
assert(helper.out_residual_idx == residual_size);
// If you're hacking, feel free to turn this off; just remember to not write off the end, and to initialize
// everything somewhere (maybe change the above to an #if 1? )
return true;
}
template <typename T>
static inline void
zldtt_ori_right(Quat<T> &orientation, xrt_quat *out)
{
struct xrt_quat tmp;
tmp.w = orientation.w;
tmp.x = orientation.x;
tmp.y = orientation.y;
tmp.z = orientation.z;
xrt_vec3 x = XRT_VEC3_UNIT_X;
xrt_vec3 z = XRT_VEC3_UNIT_Z;
math_quat_rotate_vec3(&tmp, &x, &x);
math_quat_rotate_vec3(&tmp, &z, &z);
// This is a very squashed change-of-basis from left-handed coordinate systems to right-handed coordinate
// systems: you multiply everything by (-1 0 0) then negate the X axis.
x.y *= -1;
x.z *= -1;
z.x *= -1;
math_quat_from_plus_x_z(&x, &z, out);
}
template <typename T>
static inline void
zldtt_ori_left(Quat<T> &orientation, xrt_quat *out)
{
out->w = orientation.w;
out->x = orientation.x;
out->y = orientation.y;
out->z = orientation.z;
}
template <typename T>
static inline void
zldtt(Vec3<T> &trans, Quat<T> &orientation, bool is_right, xrt_space_relation &out)
{
out.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);
out.pose.position.x = trans.x;
out.pose.position.y = trans.y;
out.pose.position.z = trans.z;
if (is_right) {
zldtt_ori_right(orientation, &out.pose.orientation);
} else {
zldtt_ori_left(orientation, &out.pose.orientation);
}
}
static void
eval_to_viz_hand(KinematicHandLM &state, xrt_hand_joint_set &out_viz_hand)
{
XRT_TRACE_MARKER();
//!@todo It's _probably_ fine to have the bigger size?
Eigen::Matrix<HandScalar, calc_input_size(true), 1> pose = state.TinyOptimizerInput.cast<HandScalar>();
OptimizerHand<HandScalar> opt = {};
OptimizerHandInit(opt, state.last_frame_pre_rotation);
OptimizerHandUnpackFromVector(pose.data(), state.optimize_hand_size, state.target_hand_size, opt);
Translations55<HandScalar> translations_absolute;
Orientations54<HandScalar> orientations_absolute;
// Vec3<HandScalar> translations_absolute[kNumFingers][kNumJointsInFinger];
// Quat<HandScalar> orientations_absolute[kNumFingers][kNumOrientationsInFinger];
eval_hand_with_orientation(opt, state.is_right, translations_absolute, orientations_absolute);
Quat<HandScalar> post_wrist_orientation;
AngleAxisToQuaternion(opt.wrist_post_orientation_aax, post_wrist_orientation);
Quat<HandScalar> pre_wrist_orientation = state.last_frame_pre_rotation;
// for (int i = 0; i < 4; i++) {
// pre_wrist_orientation[i] = state.last_frame_pre_rotation[i];
// }
Quat<HandScalar> final_wrist_orientation;
QuaternionProduct(pre_wrist_orientation, post_wrist_orientation, final_wrist_orientation);
int joint_acc_idx = 0;
// Palm.
Vec3<HandScalar> palm_position;
palm_position.x = (translations_absolute.t[2][0].x + translations_absolute.t[2][1].x) / 2;
palm_position.y = (translations_absolute.t[2][0].y + translations_absolute.t[2][1].y) / 2;
palm_position.z = (translations_absolute.t[2][0].z + translations_absolute.t[2][1].z) / 2;
Quat<HandScalar> &palm_orientation = orientations_absolute.q[2][0];
zldtt(palm_position, palm_orientation, state.is_right,
out_viz_hand.values.hand_joint_set_default[joint_acc_idx++].relation);
// Wrist.
zldtt(opt.wrist_location, final_wrist_orientation, state.is_right,
out_viz_hand.values.hand_joint_set_default[joint_acc_idx++].relation);
for (int finger = 0; finger < 5; finger++) {
for (int joint = 0; joint < 5; joint++) {
// This one is necessary
if (finger == 0 && joint == 0) {
continue;
}
Quat<HandScalar> *orientation;
if (joint != 4) {
orientation = &orientations_absolute.q[finger][joint];
} else {
orientation = &orientations_absolute.q[finger][joint - 1];
}
zldtt(translations_absolute.t[finger][joint], *orientation, state.is_right,
out_viz_hand.values.hand_joint_set_default[joint_acc_idx++].relation);
}
}
out_viz_hand.is_active = true;
}
//!@todo
// This reprojection error thing is probably the wrong way of doing it.
// I think that the right way is to hack (?) TinySolver such that we get the last set of residuals back, and use those
// somehow. Maybe scale them simply by the spread of the input hand rays? What about handling stereographic projections?
// worth it? The main bit is that we should just use the last set of residuals somehow, calculating all this is a waste
// of cycles when we have something that already should work.
static void
repro_error_make_joint_ray(const xrt_hand_joint_value &joint, const xrt_pose &cam_pose, xrt_vec3 &out_ray)
{
// by VALUE
xrt_vec3 position = joint.relation.pose.position;
math_quat_rotate_vec3(&cam_pose.orientation, &position, &position);
position = position + cam_pose.position;
out_ray = m_vec3_normalize(position);
}
static enum xrt_hand_joint
joint_from_21(int finger, int joint)
{
if (finger > 0) {
return xrt_hand_joint(2 + (finger * 5) + joint);
} else {
return xrt_hand_joint(joint + 3);
}
}
static inline float
simple_vec3_difference(const xrt_vec3 one, const xrt_vec3 two)
{
return (1.0 - m_vec3_dot(one, two));
}
float
reprojection_error_2d(KinematicHandLM &state, const one_frame_input &observation, const xrt_hand_joint_set &set)
{
float final_err = 0;
int views_looked_at = 0;
for (int view = 0; view < 2; view++) {
if (!observation.views[view].active) {
continue;
}
views_looked_at++;
xrt_pose move_amount;
if (view == 0) {
// left camera.
move_amount = XRT_POSE_IDENTITY;
} else {
move_amount = state.left_in_right;
}
xrt_vec3 lm_rays[21];
const xrt_vec3 *obs_rays = observation.views[view].rays;
int acc_idx = 0;
repro_error_make_joint_ray(set.values.hand_joint_set_default[XRT_HAND_JOINT_WRIST], move_amount,
lm_rays[acc_idx++]);
for (int finger = 0; finger < 5; finger++) {
for (int joint = 0; joint < 4; joint++) {
repro_error_make_joint_ray(
set.values.hand_joint_set_default[joint_from_21(finger, joint)], move_amount,
lm_rays[acc_idx++]);
}
}
xrt_vec3 obs_center = {};
xrt_vec3 lm_center = {};
float err = 0;
float obs_spread = 0;
float lm_spread = 0;
for (int i = 0; i < 21; i++) {
obs_center += obs_rays[i];
lm_center += lm_rays[i];
err += simple_vec3_difference(lm_rays[i], obs_rays[i]);
}
math_vec3_scalar_mul(1.0f / 21.0f, &obs_center);
math_vec3_scalar_mul(1.0f / 21.0f, &lm_center);
for (int i = 0; i < 21; i++) {
obs_spread += simple_vec3_difference(obs_center, obs_rays[i]);
}
for (int i = 0; i < 21; i++) {
lm_spread += simple_vec3_difference(lm_center, lm_rays[i]);
}
// std::cout << err << std::endl;
// std::cout << err / (obs_spread + lm_spread) << std::endl;
// return ;
final_err += err / (obs_spread + lm_spread);
}
return final_err / (float)views_looked_at;
}
template <bool optimize_hand_size>
inline float
opt_run(KinematicHandLM &state, one_frame_input &observation, xrt_hand_joint_set &out_viz_hand)
{
constexpr size_t input_size = calc_input_size(optimize_hand_size);
size_t residual_size = calc_residual_size(state.use_stability, optimize_hand_size, state.num_observation_views);
LM_DEBUG(state, "Running with %zu inputs and %zu residuals, viewed in %d cameras", input_size, residual_size,
state.num_observation_views);
CostFunctor<optimize_hand_size> cf(state, residual_size);
using AutoDiffCostFunctor =
ceres::TinySolverAutoDiffFunction<CostFunctor<optimize_hand_size>, Eigen::Dynamic, input_size, HandScalar>;
AutoDiffCostFunctor f(cf);
// Okay I have no idea if this should be {}-initialized or not. Previous me seems to have thought no, but it
// works either way.
ceres::TinySolver<AutoDiffCostFunctor> solver = {};
solver.options.max_num_iterations = 50;
// We need to do a parameter sweep for the trust region and see what's fastest.
// solver.options.initial_trust_region_radius = 1e3;
solver.options.function_tolerance = 1e-6;
Eigen::Matrix<HandScalar, input_size, 1> inp = state.TinyOptimizerInput.head<input_size>();
XRT_MAYBE_UNUSED uint64_t start = os_monotonic_get_ns();
XRT_MAYBE_UNUSED auto summary = solver.Solve(f, &inp);
XRT_MAYBE_UNUSED uint64_t end = os_monotonic_get_ns();
//!@todo Is there a zero-copy way of doing this?
state.TinyOptimizerInput.head<input_size>() = inp;
if (state.log_level <= U_LOGGING_DEBUG) {
uint64_t diff = end - start;
double time_taken = (double)diff / (double)U_TIME_1MS_IN_NS;
const char *status;
switch (summary.status) {
case 0: {
status = "GRADIENT_TOO_SMALL";
} break;
case 1: {
status = "RELATIVE_STEP_SIZE_TOO_SMALL";
} break;
case 2: {
status = "COST_TOO_SMALL";
} break;
case 3: {
status = "HIT_MAX_ITERATIONS";
} break;
case 4: {
status = "COST_CHANGE_TOO_SMALL";
} break;
}
LM_DEBUG(state, "Status: %s, num_iterations %d, max_norm %E, gtol %E", status, summary.iterations,
summary.gradient_max_norm, solver.options.gradient_tolerance);
LM_DEBUG(state, "Took %f ms", time_taken);
if (summary.iterations < 3) {
LM_DEBUG(state, "Suspiciouisly low number of iterations!");
}
}
return 0;
}
void
hand_was_untracked(KinematicHandLM *hand)
{
hand->first_frame = true;
hand->last_frame_pre_rotation.w = 1.0;
hand->last_frame_pre_rotation.x = 0.0;
hand->last_frame_pre_rotation.y = 0.0;
hand->last_frame_pre_rotation.z = 0.0;
OptimizerHandInit(hand->last_frame, hand->last_frame_pre_rotation);
OptimizerHandPackIntoVector(hand->last_frame, hand->optimize_hand_size, hand->TinyOptimizerInput.data());
}
void
optimizer_run(KinematicHandLM *hand,
one_frame_input &observation,
bool hand_was_untracked_last_frame,
bool optimize_hand_size,
float target_hand_size,
float hand_size_err_mul,
xrt_hand_joint_set &out_viz_hand,
float &out_hand_size,
float &out_reprojection_error) // NOLINT(bugprone-easily-swappable-parameters)
{
KinematicHandLM &state = *hand;
if (hand_was_untracked_last_frame) {
hand_was_untracked(hand);
}
state.num_observation_views = 0;
for (int i = 0; i < 2; i++) {
if (observation.views[i].active) {
state.num_observation_views++;
}
}
state.optimize_hand_size = optimize_hand_size;
state.target_hand_size = target_hand_size;
state.hand_size_err_mul = hand_size_err_mul;
state.use_stability = !state.first_frame;
state.observation = &observation;
for (int i = 0; i < 21; i++) {
for (int view = 0; view < 2; view++) {
unit_xrt_vec3_stereographic_projection(observation.views[view].rays[i], state.sgo[view].obs[i]);
}
}
// For now, we have to statically instantiate different versions of the optimizer depending on how many input
// parameters there are. For now, there are only two cases - either we are optimizing the hand size or we are
// not optimizing it.
// !@todo Can we make a magic template that automatically instantiates the right one, and also make it so we can
// decide to either make the residual size dynamic or static? Currently, it's dynamic, which is easier for us
// and makes compile times a lot lower, but it probably makes things some amount slower at runtime.
if (optimize_hand_size) {
opt_run<true>(state, observation, out_viz_hand);
} else {
opt_run<false>(state, observation, out_viz_hand);
}
// Postfix - unpack,
OptimizerHandUnpackFromVector(state.TinyOptimizerInput.data(), state.optimize_hand_size, state.target_hand_size,
state.last_frame);
// Squash the orientations
OptimizerHandSquashRotations(state.last_frame, state.last_frame_pre_rotation);
// Repack - brings the curl values back into original domain. Look at ModelToLM/LMToModel, we're using sin/asin.
OptimizerHandPackIntoVector(state.last_frame, hand->optimize_hand_size, state.TinyOptimizerInput.data());
eval_to_viz_hand(state, out_viz_hand);
state.first_frame = false;
out_hand_size = state.last_frame.hand_size;
out_reprojection_error = reprojection_error_2d(state, observation, out_viz_hand);
}
void
optimizer_create(xrt_pose left_in_right, bool is_right, u_logging_level log_level, KinematicHandLM **out_kinematic_hand)
{
KinematicHandLM *hand = new KinematicHandLM;
hand->is_right = is_right;
hand->left_in_right = left_in_right;
hand->log_level = log_level;
hand->left_in_right_translation.x = left_in_right.position.x;
hand->left_in_right_translation.y = left_in_right.position.y;
hand->left_in_right_translation.z = left_in_right.position.z;
hand->left_in_right_orientation.w = left_in_right.orientation.w;
hand->left_in_right_orientation.x = left_in_right.orientation.x;
hand->left_in_right_orientation.y = left_in_right.orientation.y;
hand->left_in_right_orientation.z = left_in_right.orientation.z;
// Probably unnecessary.
hand_was_untracked(hand);
*out_kinematic_hand = hand;
}
void
optimizer_destroy(KinematicHandLM **hand)
{
delete *hand;
hand = NULL;
}
} // namespace xrt::tracking::hand::mercury::lm

334
src/xrt/tracking/hand/mercury/kine_lm/lm_optimizer_params_packer.inl Normal file
View file

@ -0,0 +1,334 @@
// Copyright 2022, Collabora, Ltd.
// SPDX-License-Identifier: BSL-1.0
/*!
* @file
* @brief Util to reinterpret Ceres parameter vectors as hand model parameters
* @author Moses Turner <moses@collabora.com>
* @author Charlton Rodda <charlton.rodda@collabora.com>
* @ingroup tracking
*/
#include "util/u_logging.h"
#include "math/m_api.h"
#include "lm_interface.hpp"
#include "lm_defines.hpp"
#include <cmath>
#include <iostream>
// #include "lm_rotations.hpp"
namespace xrt::tracking::hand::mercury::lm {
template <typename T> struct OptimizerMetacarpalBone
{
Vec2<T> swing;
T twist;
};
template <typename T> struct OptimizerFinger
{
OptimizerMetacarpalBone<T> metacarpal;
Vec2<T> proximal_swing;
// Not Vec2.
T rots[2];
};
template <typename T> struct OptimizerThumb
{
OptimizerMetacarpalBone<T> metacarpal;
// Again not Vec2.
T rots[2];
};
template <typename T> struct OptimizerHand
{
T hand_size;
Vec3<T> wrist_location;
// This is constant, a ceres::Rotation.h quat,, taken from last frame.
Quat<T> wrist_pre_orientation_quat;
// This is optimized - angle-axis rotation vector. Starts at 0, loss goes up the higher it goes because it
// indicates more of a rotation.
Vec3<T> wrist_post_orientation_aax;
OptimizerThumb<T> thumb = {};
OptimizerFinger<T> finger[4] = {};
};
struct minmax
{
HandScalar min = 0;
HandScalar max = 0;
};
class FingerLimit
{
public:
minmax mcp_swing_x = {};
minmax mcp_swing_y = {};
minmax mcp_twist = {};
minmax pxm_swing_x = {};
minmax pxm_swing_y = {};
minmax curls[2] = {}; // int, dst
};
class HandLimit
{
public:
minmax hand_size;
minmax thumb_mcp_swing_x, thumb_mcp_swing_y, thumb_mcp_twist;
minmax thumb_curls[2];
FingerLimit fingers[4];
HandLimit()
{
hand_size = {0.095 - 0.03, 0.095 + 0.03};
thumb_mcp_swing_x = {rad<HandScalar>(-60), rad<HandScalar>(60)};
thumb_mcp_swing_y = {rad<HandScalar>(-60), rad<HandScalar>(60)};
thumb_mcp_twist = {rad<HandScalar>(-35), rad<HandScalar>(35)};
for (int i = 0; i < 2; i++) {
thumb_curls[i] = {rad<HandScalar>(-90), rad<HandScalar>(40)};
}
constexpr double margin = 0.09;
fingers[0].mcp_swing_y = {-0.19 - margin, -0.19 + margin};
fingers[1].mcp_swing_y = {0.00 - margin, 0.00 + margin};
fingers[2].mcp_swing_y = {0.19 - margin, 0.19 + margin};
fingers[3].mcp_swing_y = {0.38 - margin, 0.38 + margin};
for (int finger_idx = 0; finger_idx < 4; finger_idx++) {
FingerLimit &finger = fingers[finger_idx];
finger.mcp_swing_x = {rad<HandScalar>(-10), rad<HandScalar>(10)};
finger.mcp_twist = {rad<HandScalar>(-4), rad<HandScalar>(4)};
finger.pxm_swing_x = {rad<HandScalar>(-100), rad<HandScalar>(20)}; // ??? why is it reversed
finger.pxm_swing_y = {rad<HandScalar>(-20), rad<HandScalar>(20)};
for (int i = 0; i < 2; i++) {
finger.curls[i] = {rad<HandScalar>(-90), rad<HandScalar>(10)};
}
}
}
};
static const class HandLimit the_limit = {};
constexpr HandScalar hand_size_min = 0.095 - 0.03;
constexpr HandScalar hand_size_max = 0.095 + 0.03;
template <typename T>
inline T
LMToModel(T lm, minmax mm)
{
return mm.min + ((sin(lm) + T(1)) * ((mm.max - mm.min) * T(.5)));
}
template <typename T>
inline T
ModelToLM(T model, minmax mm)
{
return asin((2 * (model - mm.min) / (mm.max - mm.min)) - 1);
}
// Input vector,
template <typename T>
void
OptimizerHandUnpackFromVector(const T *in, bool use_hand_size, T hand_size, OptimizerHand<T> &out)
{
size_t acc_idx = 0;
#ifdef USE_HAND_TRANSLATION
out.wrist_location.x = in[acc_idx++];
out.wrist_location.y = in[acc_idx++];
out.wrist_location.z = in[acc_idx++];
#endif
#ifdef USE_HAND_ORIENTATION
out.wrist_post_orientation_aax.x = in[acc_idx++];
out.wrist_post_orientation_aax.y = in[acc_idx++];
out.wrist_post_orientation_aax.z = in[acc_idx++];
#endif
#ifdef USE_EVERYTHING_ELSE
out.thumb.metacarpal.swing.x = LMToModel(in[acc_idx++], the_limit.thumb_mcp_swing_x);
out.thumb.metacarpal.swing.y = LMToModel(in[acc_idx++], the_limit.thumb_mcp_swing_y);
out.thumb.metacarpal.twist = LMToModel(in[acc_idx++], the_limit.thumb_mcp_twist);
out.thumb.rots[0] = LMToModel(in[acc_idx++], the_limit.thumb_curls[0]);
out.thumb.rots[1] = LMToModel(in[acc_idx++], the_limit.thumb_curls[1]);
for (int finger_idx = 0; finger_idx < 4; finger_idx++) {
out.finger[finger_idx].metacarpal.swing.x =
LMToModel(in[acc_idx++], the_limit.fingers[finger_idx].mcp_swing_x);
out.finger[finger_idx].metacarpal.swing.y =
LMToModel(in[acc_idx++], the_limit.fingers[finger_idx].mcp_swing_y);
out.finger[finger_idx].metacarpal.twist =
LMToModel(in[acc_idx++], the_limit.fingers[finger_idx].mcp_twist);
out.finger[finger_idx].proximal_swing.x =
LMToModel(in[acc_idx++], the_limit.fingers[finger_idx].pxm_swing_x);
out.finger[finger_idx].proximal_swing.y =
LMToModel(in[acc_idx++], the_limit.fingers[finger_idx].pxm_swing_y);
out.finger[finger_idx].rots[0] = LMToModel(in[acc_idx++], the_limit.fingers[finger_idx].curls[0]);
out.finger[finger_idx].rots[1] = LMToModel(in[acc_idx++], the_limit.fingers[finger_idx].curls[1]);
}
#endif
#ifdef USE_HAND_SIZE
if (use_hand_size) {
out.hand_size = LMToModel(in[acc_idx++], the_limit.hand_size);
} else {
out.hand_size = hand_size;
}
#endif
}
template <typename T>
void
OptimizerHandPackIntoVector(OptimizerHand<T> &in, bool use_hand_size, T *out)
{
size_t acc_idx = 0;
#ifdef USE_HAND_TRANSLATION
out[acc_idx++] = in.wrist_location.x;
out[acc_idx++] = in.wrist_location.y;
out[acc_idx++] = in.wrist_location.z;
#endif
#ifdef USE_HAND_ORIENTATION
out[acc_idx++] = in.wrist_post_orientation_aax.x;
out[acc_idx++] = in.wrist_post_orientation_aax.y;
out[acc_idx++] = in.wrist_post_orientation_aax.z;
#endif
#ifdef USE_EVERYTHING_ELSE
out[acc_idx++] = ModelToLM(in.thumb.metacarpal.swing.x, the_limit.thumb_mcp_swing_x);
out[acc_idx++] = ModelToLM(in.thumb.metacarpal.swing.y, the_limit.thumb_mcp_swing_y);
out[acc_idx++] = ModelToLM(in.thumb.metacarpal.twist, the_limit.thumb_mcp_twist);
out[acc_idx++] = ModelToLM(in.thumb.rots[0], the_limit.thumb_curls[0]);
out[acc_idx++] = ModelToLM(in.thumb.rots[1], the_limit.thumb_curls[1]);
for (int finger_idx = 0; finger_idx < 4; finger_idx++) {
out[acc_idx++] =
ModelToLM(in.finger[finger_idx].metacarpal.swing.x, the_limit.fingers[finger_idx].mcp_swing_x);
out[acc_idx++] =
ModelToLM(in.finger[finger_idx].metacarpal.swing.y, the_limit.fingers[finger_idx].mcp_swing_y);
out[acc_idx++] =
ModelToLM(in.finger[finger_idx].metacarpal.twist, the_limit.fingers[finger_idx].mcp_twist);
out[acc_idx++] =
ModelToLM(in.finger[finger_idx].proximal_swing.x, the_limit.fingers[finger_idx].pxm_swing_x);
out[acc_idx++] =
ModelToLM(in.finger[finger_idx].proximal_swing.y, the_limit.fingers[finger_idx].pxm_swing_y);
out[acc_idx++] = ModelToLM(in.finger[finger_idx].rots[0], the_limit.fingers[finger_idx].curls[0]);
out[acc_idx++] = ModelToLM(in.finger[finger_idx].rots[1], the_limit.fingers[finger_idx].curls[1]);
}
#endif
#ifdef USE_HAND_SIZE
if (use_hand_size) {
out[acc_idx++] = ModelToLM(in.hand_size, the_limit.hand_size);
}
#endif
}
template <typename T>
void
OptimizerHandInit(OptimizerHand<T> &opt, Quat<T> &pre_rotation)
{
opt.hand_size = (T)(0.095);
opt.wrist_post_orientation_aax.x = (T)(0);
opt.wrist_post_orientation_aax.y = (T)(0);
opt.wrist_post_orientation_aax.z = (T)(0);
// opt.store_wrist_pre_orientation_quat = pre_rotation;
opt.wrist_pre_orientation_quat.w = (T)pre_rotation.w;
opt.wrist_pre_orientation_quat.x = (T)pre_rotation.x;
opt.wrist_pre_orientation_quat.y = (T)pre_rotation.y;
opt.wrist_pre_orientation_quat.z = (T)pre_rotation.z;
opt.wrist_location.x = (T)(0);
opt.wrist_location.y = (T)(0);
opt.wrist_location.z = (T)(-0.3);
for (int i = 0; i < 4; i++) {
//!@todo needed?
opt.finger[i].metacarpal.swing.x = T(0);
opt.finger[i].metacarpal.twist = T(0);
opt.finger[i].proximal_swing.x = rad<T>((T)(15));
opt.finger[i].rots[0] = rad<T>((T)(-5));
opt.finger[i].rots[1] = rad<T>((T)(-5));
}
opt.thumb.metacarpal.swing.x = (T)(0);
opt.thumb.metacarpal.swing.y = (T)(0);
opt.thumb.metacarpal.twist = (T)(0);
opt.thumb.rots[0] = rad<T>((T)(-5));
opt.thumb.rots[1] = rad<T>((T)(-59));
opt.finger[0].metacarpal.swing.y = (T)(-0.19);
opt.finger[1].metacarpal.swing.y = (T)(0);
opt.finger[2].metacarpal.swing.y = (T)(0.19);
opt.finger[3].metacarpal.swing.y = (T)(0.38);
opt.finger[0].proximal_swing.y = (T)(-0.01);
opt.finger[1].proximal_swing.y = (T)(0);
opt.finger[2].proximal_swing.y = (T)(0.01);
opt.finger[3].proximal_swing.y = (T)(0.02);
}
// Applies the post axis-angle rotation to the pre quat, then zeroes out the axis-angle.
template <typename T>
void
OptimizerHandSquashRotations(OptimizerHand<T> &opt, Quat<T> &out_orientation)
{
// Hmmmmm, is this at all the right thing to do? :
opt.wrist_pre_orientation_quat.w = (T)out_orientation.w;
opt.wrist_pre_orientation_quat.x = (T)out_orientation.x;
opt.wrist_pre_orientation_quat.y = (T)out_orientation.y;
opt.wrist_pre_orientation_quat.z = (T)out_orientation.z;
Quat<T> &pre_rotation = opt.wrist_pre_orientation_quat;
Quat<T> post_rotation;
AngleAxisToQuaternion(opt.wrist_post_orientation_aax, post_rotation);
Quat<T> tmp_new_pre_rotation;
QuaternionProduct(pre_rotation, post_rotation, tmp_new_pre_rotation);
out_orientation.w = tmp_new_pre_rotation.w;
out_orientation.x = tmp_new_pre_rotation.x;
out_orientation.y = tmp_new_pre_rotation.y;
out_orientation.z = tmp_new_pre_rotation.z;
opt.wrist_post_orientation_aax.x = T(0);
opt.wrist_post_orientation_aax.y = T(0);
opt.wrist_post_orientation_aax.z = T(0);
}
} // namespace xrt::tracking::hand::mercury::lm

244
src/xrt/tracking/hand/mercury/kine_lm/lm_rotations.inl Normal file
View file

@ -0,0 +1,244 @@
// Copyright 2022, Google, Inc.
// Copyright 2022, Collabora, Ltd.
// SPDX-License-Identifier: BSD-3-Clause
/*!
* @file
* @brief Autodiff-safe rotations for Levenberg-Marquardt kinematic optimizer.
* Copied out of Ceres's `rotation.h` with some modifications.
* @author Kier Mierle <kier@google.com>
* @author Sameer Agarwal <sameeragarwal@google.com>
* @author Moses Turner <moses@collabora.com>
* @ingroup tracking
*/
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
//
// * Redistributions of source code must retain the above copyright notice,
// this list of conditions and the following disclaimer.
// * Redistributions in binary form must reproduce the above copyright notice,
// this list of conditions and the following disclaimer in the documentation
// and/or other materials provided with the distribution.
// * Neither the name of Google Inc. nor the names of its contributors may be
// used to endorse or promote products derived from this software without
// specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.
#pragma once
#include <algorithm>
#include <cmath>
#include <limits>
#include "assert.h"
#include "float.h"
#include "lm_defines.hpp"
#define likely(x) __builtin_expect((x), 1)
#define unlikely(x) __builtin_expect((x), 0)
namespace xrt::tracking::hand::mercury::lm {
// For debugging.
#if 0
#include <iostream>
#define assert_quat_length_1(q) \
{ \
const T scale = q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]; \
if (abs(scale - T(1.0)) > 0.001) { \
std::cout << "Length bad! " << scale << std::endl; \
assert(false); \
}; \
}
#else
#define assert_quat_length_1(q)
#endif
template <typename T>
inline void
QuaternionProduct(const Quat<T> &z, const Quat<T> &w, Quat<T> &zw)
{
// Inplace product is not supported
assert(&z != &zw);
assert(&w != &zw);
assert_quat_length_1(z);
assert_quat_length_1(w);
// clang-format off
zw.w = z.w * w.w - z.x * w.x - z.y * w.y - z.z * w.z;
zw.x = z.w * w.x + z.x * w.w + z.y * w.z - z.z * w.y;
zw.y = z.w * w.y - z.x * w.z + z.y * w.w + z.z * w.x;
zw.z = z.w * w.z + z.x * w.y - z.y * w.x + z.z * w.w;
// clang-format on
}
template <typename T>
inline void
UnitQuaternionRotatePoint(const Quat<T> &q, const Vec3<T> &pt, Vec3<T> &result)
{
// clang-format off
T uv0 = q.y * pt.z - q.z * pt.y;
T uv1 = q.z * pt.x - q.x * pt.z;
T uv2 = q.x * pt.y - q.y * pt.x;
uv0 += uv0;
uv1 += uv1;
uv2 += uv2;
result.x = pt.x + q.w * uv0;
result.y = pt.y + q.w * uv1;
result.z = pt.z + q.w * uv2;
result.x += q.y * uv2 - q.z * uv1;
result.y += q.z * uv0 - q.x * uv2;
result.z += q.x * uv1 - q.y * uv0;
// clang-format on
}
template <typename T>
inline void
UnitQuaternionRotateAndScalePoint(const Quat<T> &q, const Vec3<T> &pt, const T scale, Vec3<T> &result)
{
T uv0 = q.y * pt.z - q.z * pt.y;
T uv1 = q.z * pt.x - q.x * pt.z;
T uv2 = q.x * pt.y - q.y * pt.x;
uv0 += uv0;
uv1 += uv1;
uv2 += uv2;
result.x = pt.x + q.w * uv0;
result.y = pt.y + q.w * uv1;
result.z = pt.z + q.w * uv2;
result.x += q.y * uv2 - q.z * uv1;
result.y += q.z * uv0 - q.x * uv2;
result.z += q.x * uv1 - q.y * uv0;
result.x *= scale;
result.y *= scale;
result.z *= scale;
}
template <typename T>
inline void
AngleAxisToQuaternion(const Vec3<T> angle_axis, Quat<T> &result)
{
const T &a0 = angle_axis.x;
const T &a1 = angle_axis.y;
const T &a2 = angle_axis.z;
const T theta_squared = a0 * a0 + a1 * a1 + a2 * a2;
// For points not at the origin, the full conversion is numerically stable.
if (likely(theta_squared > T(0.0))) {
const T theta = sqrt(theta_squared);
const T half_theta = theta * T(0.5);
const T k = sin(half_theta) / theta;
result.w = cos(half_theta);
result.x = a0 * k;
result.y = a1 * k;
result.z = a2 * k;
} else {
// At the origin, sqrt() will produce NaN in the derivative since
// the argument is zero. By approximating with a Taylor series,
// and truncating at one term, the value and first derivatives will be
// computed correctly when Jets are used.
const T k(0.5);
result.w = T(1.0);
result.x = a0 * k;
result.y = a1 * k;
result.z = a2 * k;
}
}
template <typename T>
inline void
CurlToQuaternion(const T &curl, Quat<T> &result)
{
const T theta_squared = curl * curl;
// For points not at the origin, the full conversion is numerically stable.
if (likely(theta_squared > T(0.0))) {
const T theta = curl;
const T half_theta = curl * T(0.5);
const T k = sin(half_theta) / theta;
result.w = cos(half_theta);
result.x = curl * k;
result.y = T(0.0);
result.z = T(0.0);
} else {
// At the origin, dividing by 0 is probably bad. By approximating with a Taylor series,
// and truncating at one term, the value and first derivatives will be
// computed correctly when Jets are used.
const T k(0.5);
result.w = T(1.0);
result.x = curl * k;
result.y = T(0.0);
result.z = T(0.0);
}
}
template <typename T>
inline void
SwingToQuaternion(const Vec2<T> swing, Quat<T> &result)
{
const T &a0 = swing.x;
const T &a1 = swing.y;
const T theta_squared = a0 * a0 + a1 * a1;
// For points not at the origin, the full conversion is numerically stable.
if (likely(theta_squared > T(0.0))) {
const T theta = sqrt(theta_squared);
const T half_theta = theta * T(0.5);
const T k = sin(half_theta) / theta;
result.w = cos(half_theta);
result.x = a0 * k;
result.y = a1 * k;
result.z = T(0);
} else {
// At the origin, sqrt() will produce NaN in the derivative since
// the argument is zero. By approximating with a Taylor series,
// and truncating at one term, the value and first derivatives will be
// computed correctly when Jets are used.
const T k(0.5);
result.w = T(1.0);
result.x = a0 * k;
result.y = a1 * k;
result.z = T(0);
}
}
template <typename T>
inline void
SwingTwistToQuaternion(const Vec2<T> swing, const T twist, Quat<T> &result)
{
//!@todo
// Rather than doing compound operations, we should derive it and collapse them.
Quat<T> swing_quat;
Quat<T> twist_quat;
Vec3<T> aax_twist;
aax_twist.x = (T)(0);
aax_twist.y = (T)(0);
aax_twist.z = twist;
SwingToQuaternion(swing, swing_quat);
AngleAxisToQuaternion(aax_twist, twist_quat);
QuaternionProduct(swing_quat, twist_quat, result);
}
} // namespace xrt::tracking::hand::mercury::lm