diff --git a/src/xrt/tracking/hand/mercury/CMakeLists.txt b/src/xrt/tracking/hand/mercury/CMakeLists.txt index 7c22d38b2..916248fb4 100644 --- a/src/xrt/tracking/hand/mercury/CMakeLists.txt +++ b/src/xrt/tracking/hand/mercury/CMakeLists.txt @@ -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} +) \ No newline at end of file diff --git a/src/xrt/tracking/hand/mercury/hg_image_math.hpp b/src/xrt/tracking/hand/mercury/hg_image_math.hpp deleted file mode 100644 index 477607c76..000000000 --- a/src/xrt/tracking/hand/mercury/hg_image_math.hpp +++ /dev/null @@ -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 - * @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 -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(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(0, 0); - float n_y = out_ray.at(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 diff --git a/src/xrt/tracking/hand/mercury/hg_image_math.inl b/src/xrt/tracking/hand/mercury/hg_image_math.inl new file mode 100644 index 000000000..11b01f658 --- /dev/null +++ b/src/xrt/tracking/hand/mercury/hg_image_math.inl @@ -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 + * @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 +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(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(0, 0); + float n_y = out_ray.at(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 diff --git a/src/xrt/tracking/hand/mercury/hg_model.hpp b/src/xrt/tracking/hand/mercury/hg_model.cpp similarity index 87% rename from src/xrt/tracking/hand/mercury/hg_model.hpp rename to src/xrt/tracking/hand/mercury/hg_model.cpp index dafa44195..b42b1bc5e 100644 --- a/src/xrt/tracking/hand/mercury/hg_model.hpp +++ b/src/xrt/tracking/hand/mercury/hg_model.cpp @@ -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 @@ -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(0, 0) == 0) { + U_LOG_W("Got image with zero standard deviation!"); + return; + } + data_out *= 0.25 / stddev.at(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(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++) { diff --git a/src/xrt/tracking/hand/mercury/hg_sync.cpp b/src/xrt/tracking/hand/mercury/hg_sync.cpp index afc6d2527..bdc5a4695 100644 --- a/src/xrt/tracking/hand/mercury/hg_sync.cpp +++ b/src/xrt/tracking/hand/mercury/hg_sync.cpp @@ -4,65 +4,45 @@ * @file * @brief Mercury hand tracking main file. * @author Jakob Bornecrantz - * @author Moses Turner + * @author Moses Turner + * @author Charlton Rodda * @author Nick Klingensmith * @ingroup tracking */ #include "hg_sync.hpp" -#include "hg_model.hpp" -#include "util/u_sink.h" +#include "hg_image_math.inl" +#include "math/m_vec2.h" +#include "util/u_misc.h" + + #include + namespace xrt::tracking::hand::mercury { +#define DEG_TO_RAD(DEG) (DEG * M_PI / 180.) + +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) + // Flags to tell state tracker that these are indeed valid joints static const enum xrt_space_relation_flags valid_flags_ht = (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); -static void -hgJointDisparityMath(struct HandTracking *hgt, Hand2D *hand_in_left, Hand2D *hand_in_right, Hand3D *out_hand) -{ - for (int i = 0; i < 21; i++) { - // Believe it or not, this is where the 3D stuff happens! - float t = hgt->baseline / (hand_in_left->kps[i].x - hand_in_right->kps[i].x); - - out_hand->kps[i].z = -t; - - out_hand->kps[i].x = (hand_in_left->kps[i].x * t); - out_hand->kps[i].y = -hand_in_left->kps[i].y * t; - - out_hand->kps[i].x += hgt->baseline + (hand_in_right->kps[i].x * t); - out_hand->kps[i].y += -hand_in_right->kps[i].y * t; - - out_hand->kps[i].x *= .5; - out_hand->kps[i].y *= .5; - } -} - /*! * Setup helper functions. */ -static bool -getCalibration(struct HandTracking *hgt, t_stereo_camera_calibration *calibration) + +//!@todo +// This is cruft, necessary because some users still use HT_OUTPUT_SPACE_CENTER_OF_STEREO_CAMERA. +// WMR especially should *not* be using it, and for custom North Star/ +static void +get_left_camera_to_center(struct HandTracking *hgt, xrt::auxiliary::tracking::StereoCameraCalibrationWrapper &wrap) { - xrt::auxiliary::tracking::StereoCameraCalibrationWrapper wrap(calibration); - xrt_vec3 trans = {(float)wrap.camera_translation_mat(0, 0), (float)wrap.camera_translation_mat(1, 0), - (float)wrap.camera_translation_mat(2, 0)}; - hgt->baseline = m_vec3_len(trans); - HG_DEBUG(hgt, "I think the baseline is %f meters!", hgt->baseline); - // Note, this assumes camera 0 is the left camera and camera 1 is the right camera. - // If you find one with the opposite arrangement, you'll need to invert hgt->baseline, and look at - // hgJointDisparityMath - - hgt->use_fisheye = wrap.view[0].use_fisheye; - - if (hgt->use_fisheye) { - HG_DEBUG(hgt, "I think the cameras are fisheye!"); - } else { - HG_DEBUG(hgt, "I think the cameras are not fisheye!"); - } + cv::Matx33d R1; + cv::Matx33d R2; cv::Matx34d P1; cv::Matx34d P2; @@ -105,34 +85,12 @@ getCalibration(struct HandTracking *hgt, t_stereo_camera_calibration *calibratio NULL); // validPixROI2 } - //* Good enough guess that view 0 and view 1 are the same size. - for (int i = 0; i < 2; i++) { - hgt->views[i].cameraMatrix = wrap.view[i].intrinsics_mat; + // HG_DEBUG(hgt, "R%d ->", i); + // std::cout << hgt->views[i].rotate_camera_to_stereo_camera << std::endl; - if (hgt->use_fisheye) { - hgt->views[i].distortion = wrap.view[i].distortion_fisheye_mat; - } else { - hgt->views[i].distortion = wrap.view[i].distortion_mat; - } - - if (hgt->log_level <= U_LOGGING_DEBUG) { - HG_DEBUG(hgt, "R%d ->", i); - std::cout << hgt->views[i].rotate_camera_to_stereo_camera << std::endl; - - HG_DEBUG(hgt, "K%d ->", i); - std::cout << hgt->views[i].cameraMatrix << std::endl; - - HG_DEBUG(hgt, "D%d ->", i); - std::cout << hgt->views[i].distortion << std::endl; - } - } - - hgt->calibration_one_view_size_px.w = wrap.view[0].image_size_pixels.w; - hgt->calibration_one_view_size_px.h = wrap.view[0].image_size_pixels.h; - - hgt->last_frame_one_view_size_px = hgt->calibration_one_view_size_px; - hgt->multiply_px_coord_for_undistort = 1.0f; + // Untested, so far. + //!@todo Definitely this or getCalibration are doing a transpose and I don't know which. cv::Matx33d rotate_stereo_camera_to_left_camera = hgt->views[0].rotate_camera_to_stereo_camera.inv(); xrt_matrix_3x3 s; @@ -148,15 +106,100 @@ getCalibration(struct HandTracking *hgt, t_stereo_camera_calibration *calibratio s.v[7] = rotate_stereo_camera_to_left_camera(2, 1); s.v[8] = rotate_stereo_camera_to_left_camera(2, 2); - xrt_quat tmp; - math_quat_from_matrix_3x3(&s, &tmp); + math_quat_from_matrix_3x3(&s, &hgt->hand_pose_camera_offset.orientation); + hgt->hand_pose_camera_offset.position.x = -hgt->baseline / 2; +} + +static bool +getCalibration(struct HandTracking *hgt, t_stereo_camera_calibration *calibration) +{ + xrt::auxiliary::tracking::StereoCameraCalibrationWrapper wrap(calibration); + xrt_vec3 trans = {(float)wrap.camera_translation_mat(0, 0), (float)wrap.camera_translation_mat(1, 0), + (float)wrap.camera_translation_mat(2, 0)}; + hgt->baseline = m_vec3_len(trans); + HG_DEBUG(hgt, "I think the baseline is %f meters!", hgt->baseline); + // Note, this assumes camera 0 is the left camera and camera 1 is the right camera. + // If you find one with the opposite arrangement, you'll need to invert hgt->baseline, and look at + // hgJointDisparityMath + + hgt->use_fisheye = wrap.view[0].use_fisheye; + + if (hgt->use_fisheye) { + HG_DEBUG(hgt, "I think the cameras are fisheye!"); + } else { + HG_DEBUG(hgt, "I think the cameras are not fisheye!"); + } + { + // Officially, I have no idea if this is row-major or col-major. Empirically it seems to work, and that + // is all I will say. + + // It might be that the below is *transposing* the matrix, I never remember if OpenCV's R is "left in + // right" or "right in left" + //!@todo + xrt_matrix_3x3 s; + s.v[0] = wrap.camera_rotation_mat(0, 0); + s.v[1] = wrap.camera_rotation_mat(1, 0); + s.v[2] = wrap.camera_rotation_mat(2, 0); + + s.v[3] = wrap.camera_rotation_mat(0, 1); + s.v[4] = wrap.camera_rotation_mat(1, 1); + s.v[5] = wrap.camera_rotation_mat(2, 1); + + s.v[6] = wrap.camera_rotation_mat(0, 2); + s.v[7] = wrap.camera_rotation_mat(1, 2); + s.v[8] = wrap.camera_rotation_mat(2, 2); + + xrt_pose left_in_right; + left_in_right.position.x = wrap.camera_translation_mat(0); + left_in_right.position.y = wrap.camera_translation_mat(1); + left_in_right.position.z = wrap.camera_translation_mat(2); + + math_quat_from_matrix_3x3(&s, &left_in_right.orientation); + left_in_right.orientation.x = -left_in_right.orientation.x; + left_in_right.position.y = -left_in_right.position.y; + left_in_right.position.z = -left_in_right.position.z; + + hgt->left_in_right = left_in_right; + + U_LOG_E("OpenXR: %f %f %f %f %f %f %f", left_in_right.position.x, left_in_right.position.z, + left_in_right.position.z, left_in_right.orientation.x, left_in_right.orientation.y, + left_in_right.orientation.z, left_in_right.orientation.w); + } + + + + //* Good enough guess that view 0 and view 1 are the same size. + for (int i = 0; i < 2; i++) { + hgt->views[i].cameraMatrix = wrap.view[i].intrinsics_mat; + + if (hgt->use_fisheye) { + hgt->views[i].distortion = wrap.view[i].distortion_fisheye_mat; + } else { + hgt->views[i].distortion = wrap.view[i].distortion_mat; + } + + if (hgt->log_level <= U_LOGGING_DEBUG) { + HG_DEBUG(hgt, "K%d ->", i); + std::cout << hgt->views[i].cameraMatrix << std::endl; + + HG_DEBUG(hgt, "D%d ->", i); + std::cout << hgt->views[i].distortion << std::endl; + } + } + + hgt->calibration_one_view_size_px.w = wrap.view[0].image_size_pixels.w; + hgt->calibration_one_view_size_px.h = wrap.view[0].image_size_pixels.h; + + hgt->last_frame_one_view_size_px = hgt->calibration_one_view_size_px; + hgt->multiply_px_coord_for_undistort = 1.0f; + + switch (hgt->output_space) { + case HT_OUTPUT_SPACE_LEFT_CAMERA: hgt->hand_pose_camera_offset = XRT_POSE_IDENTITY; break; + case HT_OUTPUT_SPACE_CENTER_OF_STEREO_CAMERA: get_left_camera_to_center(hgt, wrap); break; + } + - // Weird that I have to invert this quat, right? I think at some point - like probably just before this - I must - // have swapped row-major and col-major - remember, if you transpose a rotation matrix, you get its inverse. - // Doesn't matter that I don't understand - non-inverted looks definitely wrong, inverted looks definitely - // right. - math_quat_invert(&tmp, &hgt->stereo_camera_to_left_camera); return true; } @@ -199,33 +242,6 @@ getModelsFolder(struct HandTracking *hgt) #endif } - -static void -applyThumbIndexDrag(Hand3D *hand) -{ - // TERRIBLE HACK. - // Puts the thumb and pointer a bit closer together to be better at triggering XR clients' pinch detection. - static const float max_radius = 0.05; - static const float min_radius = 0.00; - - // no min drag, min drag always 0. - static const float max_drag = 0.85f; - - xrt_vec3 thumb = hand->kps[THMB_TIP]; - xrt_vec3 index = hand->kps[INDX_TIP]; - xrt_vec3 ttp = index - thumb; - float length = m_vec3_len(ttp); - if ((length > max_radius)) { - return; - } - - - float amount = math_map_ranges(length, min_radius, max_radius, max_drag, 0.0f); - - hand->kps[THMB_TIP] = m_vec3_lerp(thumb, index, amount * 0.5f); - hand->kps[INDX_TIP] = m_vec3_lerp(index, thumb, amount * 0.5f); -} - static void applyJointWidths(struct HandTracking *hgt, struct xrt_hand_joint_set *set) { @@ -258,6 +274,146 @@ applyJointWidths(struct HandTracking *hgt, struct xrt_hand_joint_set *set) .040f * .5f; // Measured my wrist thickness with calipers } +template +static inline bool +check_outside_view(struct HandTracking *hgt, struct hand_tracking_image_boundary_info_one_view boundary, Vec &keypoint) +{ + // Regular case - the keypoint is literally outside the image + if (keypoint.y > hgt->calibration_one_view_size_px.h || // + keypoint.y < 0 || // + keypoint.x > hgt->calibration_one_view_size_px.w || // + keypoint.x < 0) { + return true; + } + + switch (boundary.type) { + // No boundary, and we passed the previous check. Not outside the view. + case HT_IMAGE_BOUNDARY_NONE: return false; break; + case HT_IMAGE_BOUNDARY_CIRCLE: { + //!@optimize Most of this can be calculated once at startup + xrt_vec2 center_px = { + boundary.boundary.circle.normalized_center.x * (float)hgt->calibration_one_view_size_px.w, // + boundary.boundary.circle.normalized_center.y * (float)hgt->calibration_one_view_size_px.h}; + float radius_px = + boundary.boundary.circle.normalized_radius * (float)hgt->calibration_one_view_size_px.w; + + xrt_vec2 keypoint_xrt = {float(keypoint.x), float(keypoint.y)}; + + xrt_vec2 diff = center_px - keypoint_xrt; + if (m_vec2_len(diff) > radius_px) { + return true; + } + } break; + } + + return false; +} + +static void +back_project(struct HandTracking *hgt, + xrt_hand_joint_set *in, + bool also_debug_output, + xrt_vec2 centers[2], + float radii[2], + int num_outside[2]) +{ + + for (int view_idx = 0; view_idx < 2; view_idx++) { + xrt_pose move_amount; + + if (view_idx == 0) { + // left camera. + move_amount = XRT_POSE_IDENTITY; + } else { + move_amount = hgt->left_in_right; + } + std::vector pts_relative_to_camera(26); + + // Bandaid solution, doesn't quite fix things on WMR. + bool any_joint_behind_camera = false; + + for (int i = 0; i < 26; i++) { + xrt_vec3 tmp; + math_quat_rotate_vec3(&move_amount.orientation, + &in->values.hand_joint_set_default[i].relation.pose.position, &tmp); + pts_relative_to_camera[i].x = tmp.x + move_amount.position.x; + pts_relative_to_camera[i].y = tmp.y + move_amount.position.y; + pts_relative_to_camera[i].z = tmp.z + move_amount.position.z; + + pts_relative_to_camera[i].y *= -1; + pts_relative_to_camera[i].z *= -1; + + if (pts_relative_to_camera[i].z < 0) { + any_joint_behind_camera = true; + } + } + + + std::vector out(26); + //!@opencv_camera The OpenCV, it hurts + if (hgt->use_fisheye) { + cv::Affine3f aff = cv::Affine3f::Identity(); + cv::fisheye::projectPoints(pts_relative_to_camera, out, aff, hgt->views[view_idx].cameraMatrix, + hgt->views[view_idx].distortion); + } else { + cv::Affine3d aff = cv::Affine3d::Identity(); + // cv::Matx33d rvec = cv::Matx33d:: + cv::Matx33d rotation = aff.rotation(); + cv::projectPoints(pts_relative_to_camera, rotation, aff.translation(), + hgt->views[view_idx].cameraMatrix, hgt->views[view_idx].distortion, out); + } + xrt_vec2 keypoints_global[26]; + bool outside_view[26] = {}; + for (int i = 0; i < 26; i++) { + if (check_outside_view(hgt, hgt->image_boundary_info.views[view_idx], out[i]) || + any_joint_behind_camera) { + outside_view[i] = true; + if (num_outside != NULL) { + num_outside[view_idx]++; + } + } + keypoints_global[i].x = out[i].x / hgt->multiply_px_coord_for_undistort; + keypoints_global[i].y = out[i].y / hgt->multiply_px_coord_for_undistort; + } + + if (centers != NULL) { + centers[view_idx] = keypoints_global[XRT_HAND_JOINT_MIDDLE_PROXIMAL]; + } + + if (radii != NULL) { + for (int i = 0; i < 26; i++) { + radii[view_idx] = + std::max(radii[view_idx], m_vec2_len(centers[view_idx] - keypoints_global[i])); + } + radii[view_idx] *= hgt->tuneable_values.dyn_radii_fac.val; + } + + + + cv::Mat debug = hgt->views[view_idx].debug_out_to_this; + if (also_debug_output) { + // 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++) { + // cv::Point the_new = {(int)keypoints_global[1 + finger * 4 + joint].x, + // (int)keypoints_global[1 + finger * 4 + joint].y}; + + // cv::line(debug, last, the_new, colors[0]); + // last = the_new; + // } + // } + + for (int i = 0; i < 26; i++) { + xrt_vec2 loc; + loc.x = keypoints_global[i].x; + loc.y = keypoints_global[i].y; + handDot(debug, loc, 2, outside_view[i] ? 0.0 : (float)(i) / 26.0, 1, 2); + } + } + } // for view_idx +} + + static bool handle_changed_image_size(HandTracking *hgt, xrt_size &new_one_view_size) { @@ -283,6 +439,258 @@ handle_changed_image_size(HandTracking *hgt, xrt_size &new_one_view_size) return true; } +float +hand_confidence_value(float reprojection_error, one_frame_input &input) +{ + float out_confidence = 0.0f; + for (int view_idx = 0; view_idx < 2; view_idx++) { + for (int i = 0; i < 21; i++) { + out_confidence += input.views[view_idx].confidences[i]; + } + } + out_confidence /= 42.0f; // number of hand joints + float reproj_err_mul = 1.0f / (reprojection_error + 1.0f); + return out_confidence * reproj_err_mul; +} + + +xrt_vec3 +correct_direction(xrt_vec2 in) +{ + xrt_vec3 out = {in.x, -in.y, -1}; + return m_vec3_normalize(out); +} + +void +check_new_user_event(struct HandTracking *hgt) +{ + if (hgt->tuneable_values.new_user_event) { + hgt->tuneable_values.new_user_event = false; + hgt->hand_seen_before[0] = false; + hgt->hand_seen_before[1] = false; + hgt->refinement.hand_size_refinement_schedule_x = 0; + } +} + +bool +should_run_detection(struct HandTracking *hgt) +{ + if (hgt->tuneable_values.always_run_detection_model) { + return true; + } else { + hgt->detection_counter++; + // Every 30 frames, but only if we aren't tracking both hands. + bool saw_both_hands_last_frame = hgt->last_frame_hand_detected[0] && hgt->last_frame_hand_detected[1]; + return (hgt->detection_counter % 30 == 0) && !saw_both_hands_last_frame; + } +} + +void +dispatch_and_process_hand_detections(struct HandTracking *hgt) +{ + if (hgt->tuneable_values.always_run_detection_model) { + // Pretend like nothing was detected last frame. + for (int hand_idx = 0; hand_idx < 2; hand_idx++) { + // hgt->last_frame_hand_detected[hand_idx] = false; + hgt->this_frame_hand_detected[hand_idx] = false; + + hgt->histories[hand_idx].hands.clear(); + hgt->histories[hand_idx].timestamps.clear(); + } + } + + // view, hand + hand_bounding_box states[2][2] = {}; + + // paranoia + states[0]->found = false; + states[1]->found = false; + + states[0]->confidence = 0; + states[1]->confidence = 0; + + + hand_detection_run_info infos[2] = {}; + + infos[0].view = &hgt->views[0]; + infos[1].view = &hgt->views[1]; + + infos[0].outputs[0] = &states[0][0]; + infos[0].outputs[1] = &states[0][1]; + + infos[1].outputs[0] = &states[1][0]; + infos[1].outputs[1] = &states[1][1]; + + + u_worker_group_push(hgt->group, run_hand_detection, &infos[0]); + u_worker_group_push(hgt->group, run_hand_detection, &infos[1]); + u_worker_group_wait_all(hgt->group); + for (int hand_idx = 0; hand_idx < 2; hand_idx++) { + if ((states[0][hand_idx].confidence + states[1][hand_idx].confidence) < 0.90) { + continue; + } + + + //!@todo Commented out the below code, which required all detections to be pointing at roughly the same + //! point in space. + // We should add this back, instead using lineline.cpp. But I gotta ship this, so we're just going to be + // less robust for now. + + // xrt_vec2 in_left = raycoord(&hgt->views[0], states[0][hand_idx].center); + // xrt_vec2 in_right = raycoord(&hgt->views[1], states[1][hand_idx].center); + + // xrt_vec2 dir_y_l = {in_left.y, -1.0f}; + // xrt_vec2 dir_y_r = {in_right.y, -1.0f}; + + // m_vec2_normalize(&dir_y_l); + // m_vec2_normalize(&dir_y_r); + + // float minimum = cosf(DEG_TO_RAD(10)); + + // float diff = m_vec2_dot(dir_y_l, dir_y_r); + + // // U_LOG_E("diff %f", diff); + + // if (diff < minimum) { + // HG_DEBUG(hgt, + // "Mismatch in detection models! Diff is %f, left Y axis is %f, right Y " + // "axis is %f", + // diff, in_left.y, in_right.y); + // continue; + // } + + // If this hand was not detected last frame, we can add our prediction in. + // Or, if we're running the model every frame. + if (hgt->tuneable_values.always_run_detection_model || !hgt->last_frame_hand_detected[hand_idx]) { + hgt->views[0].bboxes_this_frame[hand_idx] = states[0][hand_idx]; + hgt->views[1].bboxes_this_frame[hand_idx] = states[1][hand_idx]; + } + + + hgt->this_frame_hand_detected[hand_idx] = true; + } + // Most of the time, this codepath runs - we predict where the hand should be based on the last + // two frames. +} + +void +predict_new_regions_of_interest(struct HandTracking *hgt) +{ + + for (int hand_idx = 0; hand_idx < 2; hand_idx++) { + // If we don't have the past two frames, this code doesn't do what we want. + // If we only have *one* frame, we just reuse the same bounding box and hope the hand + // hasn't moved too much. @todo + + if (hgt->histories[hand_idx].timestamps.size() < 2) { + HG_TRACE(hgt, "continuing, size is %zu", hgt->histories[hand_idx].timestamps.size()); + continue; + } + + // We can only do this *after* we know we're predicting - this would otherwise overwrite the detection + // model. + hgt->this_frame_hand_detected[hand_idx] = hgt->last_frame_hand_detected[hand_idx]; + + hand_history &history = hgt->histories[hand_idx]; + uint64_t time_two_frames_ago = *history.timestamps.get_at_age(1); + uint64_t time_one_frame_ago = *history.timestamps.get_at_age(0); + uint64_t time_now = hgt->current_frame_timestamp; + + xrt_hand_joint_set *set_two_frames_ago = history.hands.get_at_age(1); + xrt_hand_joint_set *set_one_frame_ago = history.hands.get_at_age(0); + + // double dt_past = (double)() / (double)U_TIME_1S_IN_NS; + double dt_past = time_ns_to_s(time_one_frame_ago - time_two_frames_ago); + + double dt_now = time_ns_to_s(time_now - time_one_frame_ago); + + + xrt_vec3 vels[26]; + + for (int i = 0; i < 26; i++) { + + + xrt_vec3 from_to = set_one_frame_ago->values.hand_joint_set_default[i].relation.pose.position - + set_two_frames_ago->values.hand_joint_set_default[i].relation.pose.position; + vels[i] = m_vec3_mul_scalar(from_to, 1.0 / dt_past); + + // U_LOG_E("%f %f %f", vels[i].x, vels[i].y, vels[i].z); + } + xrt_vec3 positions_last_frame[26]; + // xrt_vec3 predicted_positions_this_frame[26]; + xrt_hand_joint_set predicted_positions_this_frame; + + for (int i = 0; i < 26; i++) { + positions_last_frame[i] = + set_one_frame_ago->values.hand_joint_set_default[i].relation.pose.position; + + //!@todo I dunno if this is right. + // Number of times this has been changed without rigorously testing: 1 + float lerp_between_last_frame_and_predicted = + hgt->tuneable_values.amount_to_lerp_prediction.val; + predicted_positions_this_frame.values.hand_joint_set_default[i].relation.pose.position = + positions_last_frame[i] + (vels[i] * dt_now * lerp_between_last_frame_and_predicted); + } + xrt_vec2 centers[2] = {}; + float radii[2] = {}; + int num_outside[2] = {0, 0}; + + back_project( // + hgt, // + &predicted_positions_this_frame, // + hgt->tuneable_values.scribble_predictions_into_this_frame && hgt->debug_scribble, // + centers, // + radii, // + num_outside); + + for (int view_idx = 0; view_idx < 2; view_idx++) { + if (num_outside[view_idx] < hgt->tuneable_values.max_num_outside_view) { + hgt->views[view_idx].bboxes_this_frame[hand_idx].center = centers[view_idx]; + hgt->views[view_idx].bboxes_this_frame[hand_idx].size_px = radii[view_idx]; + hgt->views[view_idx].bboxes_this_frame[hand_idx].found = true; + } else { + hgt->views[view_idx].bboxes_this_frame[hand_idx].found = false; + } + } + } + + if (hgt->debug_scribble) { + + for (int view_idx = 0; view_idx < 2; view_idx++) { + for (int hand_idx = 0; hand_idx < 2; hand_idx++) { + if (!hgt->views[view_idx].bboxes_this_frame[hand_idx].found) { + continue; + } + xrt_vec2 &_pt = hgt->views[view_idx].bboxes_this_frame[hand_idx].center; + float &size = hgt->views[view_idx].bboxes_this_frame[hand_idx].size_px; + cv::Point2i pt(_pt.x, _pt.y); + cv::rectangle(hgt->views[view_idx].debug_out_to_this, + cv::Rect(pt - cv::Point2i(size / 2, size / 2), cv::Size(size, size)), + colors[hand_idx], 1); + } + } + } +} + +void +scribble_image_boundary(struct HandTracking *hgt) +{ + for (int view_idx = 0; view_idx < 2; view_idx++) { + struct ht_view *view = &hgt->views[view_idx]; + + cv::Mat &debug_frame = view->debug_out_to_this; + hand_tracking_image_boundary_info_one_view &info = hgt->image_boundary_info.views[view_idx]; + + if (info.type == HT_IMAGE_BOUNDARY_CIRCLE) { + int center_x = hgt->last_frame_one_view_size_px.w * info.boundary.circle.normalized_center.x; + int center_y = hgt->last_frame_one_view_size_px.h * info.boundary.circle.normalized_center.y; + cv::circle(debug_frame, {center_x, center_y}, + info.boundary.circle.normalized_radius * hgt->last_frame_one_view_size_px.w, + hsv2rgb(270.0, 0.5, 0.5), 2); + } + } +} + /* * * Member functions. @@ -313,8 +721,11 @@ HandTracking::~HandTracking() t_stereo_camera_calibration_reference(&this->calib, NULL); - free_kinematic_hand(&this->kinematic_hands[0]); - free_kinematic_hand(&this->kinematic_hands[1]); + lm::optimizer_destroy(&this->kinematic_hands[0]); + lm::optimizer_destroy(&this->kinematic_hands[1]); + + ccdik::free_kinematic_hand(&this->kinematic_hands_ccdik[0]); + ccdik::free_kinematic_hand(&this->kinematic_hands_ccdik[1]); u_var_remove_root((void *)&this->base); u_frame_times_widget_teardown(&this->ft_widget); @@ -352,7 +763,7 @@ HandTracking::cCallbackProcess(struct t_hand_tracking_sync *ht_sync, xrt_size new_one_view_size; new_one_view_size.h = left_frame->height; new_one_view_size.w = left_frame->width; - // Could be an assert, should never happen. + // Could be an assert, should never happen after first frame. if (!handle_changed_image_size(hgt, new_one_view_size)) { return; } @@ -377,7 +788,7 @@ HandTracking::cCallbackProcess(struct t_hand_tracking_sync *ht_sync, cv::Mat debug_output = {}; xrt_frame *debug_frame = nullptr; - + // If we're outputting to a debug image, setup the image. if (hgt->debug_scribble) { u_frame_create_one_off(XRT_FORMAT_R8G8B8, full_width, full_height, &debug_frame); debug_frame->timestamp = hgt->current_frame_timestamp; @@ -391,25 +802,40 @@ HandTracking::cCallbackProcess(struct t_hand_tracking_sync *ht_sync, hgt->views[0].debug_out_to_this = debug_output(cv::Rect(view_offsets[0], view_size)); hgt->views[1].debug_out_to_this = debug_output(cv::Rect(view_offsets[1], view_size)); + scribble_image_boundary(hgt); } - u_worker_group_push(hgt->group, run_hand_detection, &hgt->views[0]); - u_worker_group_push(hgt->group, run_hand_detection, &hgt->views[1]); - u_worker_group_wait_all(hgt->group); - + check_new_user_event(hgt); + // Every now and then if we're not already tracking both hands, try to detect new hands. + if (should_run_detection(hgt)) { + dispatch_and_process_hand_detections(hgt); + } + // For already-tracked hands, predict where we think they should be in image space based on the past two + // frames. Note that this always happens We want to pose-predict already tracked hands but not mess with + // just-detected hands + if (!hgt->tuneable_values.always_run_detection_model) { + predict_new_regions_of_interest(hgt); + } + //!@todo does this go here? + // If no hand regions of interest were found anywhere, there's no hand - register that in the state tracker for (int hand_idx = 0; hand_idx < 2; hand_idx++) { - - if (!(hgt->views[0].det_outputs[hand_idx].found && hgt->views[1].det_outputs[hand_idx].found)) { - // If we don't find this hand in *both* views - out_xrt_hands[hand_idx]->is_active = false; - continue; + if (!(hgt->views[0].bboxes_this_frame[hand_idx].found || + hgt->views[1].bboxes_this_frame[hand_idx].found)) { + hgt->this_frame_hand_detected[hand_idx] = false; } - out_xrt_hands[hand_idx]->is_active = true; + } + // Dispatch keypoint estimator neural nets + for (int hand_idx = 0; hand_idx < 2; hand_idx++) { for (int view_idx = 0; view_idx < 2; view_idx++) { + + if (!hgt->views[view_idx].bboxes_this_frame[hand_idx].found) { + continue; + } + struct keypoint_estimation_run_info &inf = hgt->views[view_idx].run_info[hand_idx]; inf.view = &hgt->views[view_idx]; inf.hand_idx = hand_idx; @@ -419,58 +845,162 @@ HandTracking::cCallbackProcess(struct t_hand_tracking_sync *ht_sync, } u_worker_group_wait_all(hgt->group); + + // Spaghetti logic for optimizing hand size + bool any_hands_are_only_visible_in_one_view = false; + for (int hand_idx = 0; hand_idx < 2; hand_idx++) { - if (!out_xrt_hands[hand_idx]->is_active) { - hgt->last_frame_hand_detected[hand_idx] = false; + any_hands_are_only_visible_in_one_view = // + any_hands_are_only_visible_in_one_view || // + (hgt->views[0].bboxes_this_frame[hand_idx].found != // + hgt->views[1].bboxes_this_frame[hand_idx].found); + } + + constexpr float mul_max = 1.0; + constexpr float frame_max = 100; + bool optimize_hand_size; + + if ((hgt->refinement.hand_size_refinement_schedule_x > frame_max)) { + hgt->refinement.hand_size_refinement_schedule_y = mul_max; + optimize_hand_size = false; + } else { + hgt->refinement.hand_size_refinement_schedule_y = + powf((hgt->refinement.hand_size_refinement_schedule_x / frame_max), 2) * mul_max; + optimize_hand_size = true; + } + + if (any_hands_are_only_visible_in_one_view) { + optimize_hand_size = false; + } + + + // if either hand was not visible before the last new-user event but is visible now, reset the schedule + // a bit. + if ((hgt->this_frame_hand_detected[0] && !hgt->hand_seen_before[0]) || + (hgt->this_frame_hand_detected[1] && !hgt->hand_seen_before[1])) { + hgt->refinement.hand_size_refinement_schedule_x = + std::min(hgt->refinement.hand_size_refinement_schedule_x, frame_max / 2); + } + + int num_hands = 0; + float avg_hand_size = 0; + + // Dispatch the optimizers! + for (int hand_idx = 0; hand_idx < 2; hand_idx++) { + if (!hgt->this_frame_hand_detected[hand_idx]) { continue; } - kine::kinematic_hand_4f *hand = hgt->kinematic_hands[hand_idx]; - if (!hgt->last_frame_hand_detected[hand_idx]) { - kine::init_hardcoded_statics(hand, hgt->hand_size / 100.0f); + + one_frame_input input; + + for (int view = 0; view < 2; view++) { + keypoint_output *from_model = &hgt->views[view].keypoint_outputs[hand_idx]; + input.views[view].active = hgt->views[view].bboxes_this_frame[hand_idx].found; + if (!input.views[view].active) { + continue; + } + for (int i = 0; i < 21; i++) { + input.views[view].confidences[i] = from_model->hand_tan_space.confidences[i]; + // std::cout << input.views[view].confidences[i] << std::endl; + input.views[view].rays[i] = correct_direction(from_model->hand_tan_space.kps[i]); + } } - Hand2D *hand_in_left_view = &hgt->views[0].keypoint_outputs[hand_idx].hand_tan_space; - Hand2D *hand_in_right_view = &hgt->views[1].keypoint_outputs[hand_idx].hand_tan_space; - Hand3D hand_3d; - - - struct xrt_hand_joint_set *put_in_set = out_xrt_hands[hand_idx]; - applyThumbIndexDrag(&hand_3d); + if (__builtin_expect(!hgt->tuneable_values.use_ccdik, true)) { + lm::KinematicHandLM *hand = hgt->kinematic_hands[hand_idx]; + + //!@todo + // ABOUT TWO MINUTES OF THOUGHT WERE PUT INTO THIS VALUE + float reprojection_error_threshold = 0.35f; + + float out_hand_size; + + //!@optimize We can have one of these on each thread + float reprojection_error; + lm::optimizer_run(hand, // + input, // + !hgt->last_frame_hand_detected[hand_idx], // + optimize_hand_size, // + hgt->target_hand_size, // + hgt->refinement.hand_size_refinement_schedule_y, // + *put_in_set, // + out_hand_size, // + reprojection_error); + + avg_hand_size += out_hand_size; + num_hands++; + + if (reprojection_error > reprojection_error_threshold) { + HG_DEBUG(hgt, "Reprojection error above threshold!"); + hgt->this_frame_hand_detected[hand_idx] = false; + + continue; + } + if (!any_hands_are_only_visible_in_one_view) { + hgt->refinement.hand_size_refinement_schedule_x += + hand_confidence_value(reprojection_error, input); + } + + } else { + ccdik::KinematicHandCCDIK *hand = hgt->kinematic_hands_ccdik[hand_idx]; + if (!hgt->last_frame_hand_detected[hand_idx]) { + ccdik::init_hardcoded_statics(hand, hgt->target_hand_size); + } + ccdik::optimize_new_frame(hand, input, *put_in_set); + } + + applyJointWidths(hgt, put_in_set); - hgJointDisparityMath(hgt, hand_in_left_view, hand_in_right_view, &hand_3d); - - kine::optimize_new_frame(hand_3d.kps, hand, put_in_set, hand_idx); - - - math_pose_identity(&put_in_set->hand_pose.pose); - - switch (hgt->output_space) { - case HT_OUTPUT_SPACE_LEFT_CAMERA: { - put_in_set->hand_pose.pose.orientation = hgt->stereo_camera_to_left_camera; - } break; - case HT_OUTPUT_SPACE_CENTER_OF_STEREO_CAMERA: { - put_in_set->hand_pose.pose.orientation.w = 1.0; - put_in_set->hand_pose.pose.position.x = -hgt->baseline / 2; - } break; + // Just debug scribbling - remove this in hard production environment + if (hgt->tuneable_values.scribble_optimizer_outputs && hgt->debug_scribble) { + back_project(hgt, put_in_set, true, NULL, NULL, NULL); } + put_in_set->hand_pose.pose = hgt->hand_pose_camera_offset; put_in_set->hand_pose.relation_flags = valid_flags_ht; + + + hgt->histories[hand_idx].hands.push_back(*put_in_set); + hgt->histories[hand_idx].timestamps.push_back(hgt->current_frame_timestamp); } + // More hand-size-optimization spaghetti + if (num_hands > 0) { + hgt->target_hand_size = (float)avg_hand_size / (float)num_hands; + } + // State tracker tweaks + for (int hand_idx = 0; hand_idx < 2; hand_idx++) { + out_xrt_hands[hand_idx]->is_active = hgt->this_frame_hand_detected[hand_idx]; + hgt->last_frame_hand_detected[hand_idx] = hgt->this_frame_hand_detected[hand_idx]; + hgt->hand_seen_before[hand_idx] = + hgt->hand_seen_before[hand_idx] || hgt->this_frame_hand_detected[hand_idx]; + + if (!hgt->last_frame_hand_detected[hand_idx]) { + hgt->views[0].bboxes_this_frame[hand_idx].found = false; + hgt->views[1].bboxes_this_frame[hand_idx].found = false; + hgt->histories[hand_idx].hands.clear(); + hgt->histories[hand_idx].timestamps.clear(); + } + } + + // If the debug UI is active, push to the frame-timing widget u_frame_times_widget_push_sample(&hgt->ft_widget, hgt->current_frame_timestamp); + // If the debug UI is active, push our debug frame if (hgt->debug_scribble) { u_sink_debug_push_frame(&hgt->debug_sink, debug_frame); xrt_frame_reference(&debug_frame, NULL); } + + // done! } void @@ -484,6 +1014,8 @@ HandTracking::cCallbackDestroy(t_hand_tracking_sync *ht_sync) } // namespace xrt::tracking::hand::mercury +using namespace xrt::tracking::hand::mercury; + /* * * 'Exported' functions. @@ -546,6 +1078,8 @@ t_hand_tracking_sync_mercury_create(struct t_stereo_camera_calibration *calib, hgt->views[0].hgt = hgt; hgt->views[1].hgt = hgt; // :) + hgt->image_boundary_info = boundary_info; + init_hand_detection(hgt, &hgt->views[0].detection); init_hand_detection(hgt, &hgt->views[1].detection); @@ -572,18 +1106,55 @@ t_hand_tracking_sync_mercury_create(struct t_stereo_camera_calibration *calib, hgt->pool = u_worker_thread_pool_create(num_threads - 1, num_threads); hgt->group = u_worker_group_create(hgt->pool); - hgt->hand_size = 9.5864; - xrt::tracking::hand::mercury::kine::alloc_kinematic_hand(&hgt->kinematic_hands[0]); - xrt::tracking::hand::mercury::kine::alloc_kinematic_hand(&hgt->kinematic_hands[1]); + lm::optimizer_create(hgt->left_in_right, false, hgt->log_level, &hgt->kinematic_hands[0]); + lm::optimizer_create(hgt->left_in_right, true, hgt->log_level, &hgt->kinematic_hands[1]); + + ccdik::alloc_kinematic_hand(hgt->left_in_right, false, &hgt->kinematic_hands_ccdik[0]); + ccdik::alloc_kinematic_hand(hgt->left_in_right, true, &hgt->kinematic_hands_ccdik[1]); u_frame_times_widget_init(&hgt->ft_widget, 10.0f, 10.0f); u_var_add_root(hgt, "Camera-based Hand Tracker", true); - u_var_add_f32(hgt, &hgt->hand_size, "Hand size (Centimeters between wrist and middle-proximal joint)"); u_var_add_ro_f32(hgt, &hgt->ft_widget.fps, "FPS!"); - u_var_add_f32_timing(hgt, hgt->ft_widget.debug_var, "times!"); + u_var_add_f32_timing(hgt, hgt->ft_widget.debug_var, "Frame timing!"); + + u_var_add_ro_f32(hgt, &hgt->target_hand_size, "Hand size (Meters between wrist and middle-proximal joint)"); + u_var_add_ro_f32(hgt, &hgt->refinement.hand_size_refinement_schedule_x, "Schedule (X value)"); + u_var_add_ro_f32(hgt, &hgt->refinement.hand_size_refinement_schedule_y, "Schedule (Y value)"); + + + u_var_add_bool(hgt, &hgt->tuneable_values.new_user_event, "Trigger new-user event!"); + + hgt->tuneable_values.dyn_radii_fac.max = 4.0f; + hgt->tuneable_values.dyn_radii_fac.min = 0.3f; + hgt->tuneable_values.dyn_radii_fac.step = 0.02f; + hgt->tuneable_values.dyn_radii_fac.val = 3.0f; + + hgt->tuneable_values.dyn_joint_y_angle_error.max = 40.0f; + hgt->tuneable_values.dyn_joint_y_angle_error.min = 0.0f; + hgt->tuneable_values.dyn_joint_y_angle_error.step = 0.1f; + hgt->tuneable_values.dyn_joint_y_angle_error.val = 10.0f; + + // Number of times this has been changed without rigorously testing: 1 + hgt->tuneable_values.amount_to_lerp_prediction.max = 1.5f; + hgt->tuneable_values.amount_to_lerp_prediction.min = -1.5f; + hgt->tuneable_values.amount_to_lerp_prediction.step = 0.01f; + hgt->tuneable_values.amount_to_lerp_prediction.val = 0.4f; + + + u_var_add_draggable_f32(hgt, &hgt->tuneable_values.dyn_radii_fac, "radius factor (predict)"); + u_var_add_draggable_f32(hgt, &hgt->tuneable_values.dyn_joint_y_angle_error, "max error hand joint"); + u_var_add_draggable_f32(hgt, &hgt->tuneable_values.amount_to_lerp_prediction, "Amount to lerp pose-prediction"); + + u_var_add_bool(hgt, &hgt->tuneable_values.scribble_predictions_into_this_frame, "Scribble pose-predictions"); + u_var_add_bool(hgt, &hgt->tuneable_values.scribble_keypoint_model_outputs, "Scribble keypoint model output"); + u_var_add_bool(hgt, &hgt->tuneable_values.scribble_optimizer_outputs, "Scribble kinematic optimizer output"); + u_var_add_bool(hgt, &hgt->tuneable_values.always_run_detection_model, "Always run detection model"); + u_var_add_bool(hgt, &hgt->tuneable_values.use_ccdik, + "Use IK optimizer (may put tracking in unexpected state, use with care)"); + u_var_add_sink_debug(hgt, &hgt->debug_sink, "i"); diff --git a/src/xrt/tracking/hand/mercury/hg_sync.hpp b/src/xrt/tracking/hand/mercury/hg_sync.hpp index 33ddd24ec..ba1ddac64 100644 --- a/src/xrt/tracking/hand/mercury/hg_sync.hpp +++ b/src/xrt/tracking/hand/mercury/hg_sync.hpp @@ -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 #include #include @@ -41,62 +44,40 @@ #include #include -#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 hands; + HistoryBuffer 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 diff --git a/src/xrt/tracking/hand/mercury/kine/kinematic_interface.hpp b/src/xrt/tracking/hand/mercury/kine/kinematic_interface.hpp deleted file mode 100644 index 3be1999c9..000000000 --- a/src/xrt/tracking/hand/mercury/kine/kinematic_interface.hpp +++ /dev/null @@ -1,34 +0,0 @@ -// Copyright 2022, Collabora, Ltd. -// SPDX-License-Identifier: BSL-1.0 -/*! - * @file - * @brief Interface for kinematic model - * @author Moses Turner - * @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 diff --git a/src/xrt/tracking/hand/mercury/kine_ccdik/CMakeLists.txt b/src/xrt/tracking/hand/mercury/kine_ccdik/CMakeLists.txt new file mode 100644 index 000000000..f54b7c692 --- /dev/null +++ b/src/xrt/tracking/hand/mercury/kine_ccdik/CMakeLists.txt @@ -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}) diff --git a/src/xrt/tracking/hand/mercury/kine/kinematic_defines.hpp b/src/xrt/tracking/hand/mercury/kine_ccdik/ccdik_defines.hpp similarity index 69% rename from src/xrt/tracking/hand/mercury/kine/kinematic_defines.hpp rename to src/xrt/tracking/hand/mercury/kine_ccdik/ccdik_defines.hpp index bc82ce5c5..f005dc4ed 100644 --- a/src/xrt/tracking/hand/mercury/kine/kinematic_defines.hpp +++ b/src/xrt/tracking/hand/mercury/kine_ccdik/ccdik_defines.hpp @@ -35,44 +35,12 @@ #include #include +#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 t_jts_as_mat; - Eigen::Matrix kinematic; - - // // Pointers to the locations of - // struct xrt_vec3 *loc_ptrs[21]; - -} kinematic_hand_4f; + xrt_vec3 t_jts[kNumNNJoints] = {}; + Eigen::Matrix t_jts_as_mat = {}; + Eigen::Matrix kinematic = {}; +}; #define CONTINUE_IF_HIDDEN_THUMB \ @@ -208,4 +175,4 @@ typedef struct kinematic_hand_4f continue; \ } -} // namespace xrt::tracking::hand::mercury::kine \ No newline at end of file +} // namespace xrt::tracking::hand::mercury::ccdik \ No newline at end of file diff --git a/src/xrt/tracking/hand/mercury/kine/kinematic_hand_init.hpp b/src/xrt/tracking/hand/mercury/kine_ccdik/ccdik_hand_init.hpp similarity index 92% rename from src/xrt/tracking/hand/mercury/kine/kinematic_hand_init.hpp rename to src/xrt/tracking/hand/mercury/kine_ccdik/ccdik_hand_init.hpp index 562c85bbc..809eef7bc 100644 --- a/src/xrt/tracking/hand/mercury/kine/kinematic_hand_init.hpp +++ b/src/xrt/tracking/hand/mercury/kine_ccdik/ccdik_hand_init.hpp @@ -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 &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 diff --git a/src/xrt/tracking/hand/mercury/kine_ccdik/ccdik_interface.hpp b/src/xrt/tracking/hand/mercury/kine_ccdik/ccdik_interface.hpp new file mode 100644 index 000000000..37632ca75 --- /dev/null +++ b/src/xrt/tracking/hand/mercury/kine_ccdik/ccdik_interface.hpp @@ -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 + * @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 diff --git a/src/xrt/tracking/hand/mercury/kine/kinematic_main.cpp b/src/xrt/tracking/hand/mercury/kine_ccdik/ccdik_main.cpp similarity index 67% rename from src/xrt/tracking/hand/mercury/kine/kinematic_main.cpp rename to src/xrt/tracking/hand/mercury/kine_ccdik/ccdik_main.cpp index 11f4c2975..d80bb1d22 100644 --- a/src/xrt/tracking/hand/mercury/kine/kinematic_main.cpp +++ b/src/xrt/tracking/hand/mercury/kine_ccdik/ccdik_main.cpp @@ -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 #include @@ -18,17 +21,17 @@ -namespace xrt::tracking::hand::mercury::kine { +namespace xrt::tracking::hand::mercury::ccdik { static void -_two_set_ele(Eigen::Matrix &thing, Eigen::Affine3f jt, int idx) +_two_set_ele(Eigen::Matrix &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 diff --git a/src/xrt/tracking/hand/mercury/kine/kinematic_tiny_math.hpp b/src/xrt/tracking/hand/mercury/kine_ccdik/ccdik_tiny_math.hpp similarity index 77% rename from src/xrt/tracking/hand/mercury/kine/kinematic_tiny_math.hpp rename to src/xrt/tracking/hand/mercury/kine_ccdik/ccdik_tiny_math.hpp index 21f17d825..99fa60d70 100644 --- a/src/xrt/tracking/hand/mercury/kine/kinematic_tiny_math.hpp +++ b/src/xrt/tracking/hand/mercury/kine_ccdik/ccdik_tiny_math.hpp @@ -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 diff --git a/src/xrt/tracking/hand/mercury/kine_ccdik/lineline.hpp b/src/xrt/tracking/hand/mercury/kine_ccdik/lineline.hpp new file mode 100644 index 000000000..62bf745e6 --- /dev/null +++ b/src/xrt/tracking/hand/mercury/kine_ccdik/lineline.hpp @@ -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 + * @author Moses Turner + * @ingroup tracking + */ +#include "float.h" +#include +#include +#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); +} diff --git a/src/xrt/tracking/hand/mercury/kine_common.hpp b/src/xrt/tracking/hand/mercury/kine_common.hpp new file mode 100644 index 000000000..4f4d807e3 --- /dev/null +++ b/src/xrt/tracking/hand/mercury/kine_common.hpp @@ -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 + * @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 diff --git a/src/xrt/tracking/hand/mercury/kine_lm/CMakeLists.txt b/src/xrt/tracking/hand/mercury/kine_lm/CMakeLists.txt new file mode 100644 index 000000000..1326efa55 --- /dev/null +++ b/src/xrt/tracking/hand/mercury/kine_lm/CMakeLists.txt @@ -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} +) \ No newline at end of file diff --git a/src/xrt/tracking/hand/mercury/kine_lm/lm_defines.hpp b/src/xrt/tracking/hand/mercury/kine_lm/lm_defines.hpp new file mode 100644 index 000000000..eb4541705 --- /dev/null +++ b/src/xrt/tracking/hand/mercury/kine_lm/lm_defines.hpp @@ -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 + * @ingroup tracking + */ +#pragma once + +// #include +// #include +#include "math/m_mathinclude.h" +#include "../kine_common.hpp" +#include + +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 +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 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) = default; + + /// Move constructor + Quat(Quat &&) noexcept(std::is_nothrow_move_constructible_v) = default; + + /// Copy assignment + Quat & + operator=(Quat const &) = default; + + /// Move assignment + Quat & + operator=(Quat &&) noexcept = default; + + /// Construct from x, y, z, w scalars + template + 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 Quat(Quat 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 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) = default; + + /// Move constructor + Vec3(Vec3 &&) noexcept(std::is_nothrow_move_constructible_v) = default; + + /// Copy assignment + Vec3 & + operator=(Vec3 const &) = default; + + /// Move assignment + Vec3 & + operator=(Vec3 &&) noexcept = default; + + + template + constexpr Vec3(Other x, Other y, Other z) noexcept // NOLINT(bugprone-easily-swappable-parameters) + : x{Scalar(x)}, y{Scalar(y)}, z{Scalar(z)} + {} + + template Vec3(Vec3 const &other) : Vec3(other.x, other.y, other.z) {} + + static Vec3 + Zero() + { + return Vec3(0.f, 0.f, 0.f); + } +}; + +template 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) = default; + + /// Move constructor + constexpr Vec2(Vec2 &&) noexcept(std::is_nothrow_move_constructible_v) = 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 + Vec2(Other x, Other y) // NOLINT(bugprone-easily-swappable-parameters) + noexcept(std::is_nothrow_constructible_v) + : x{Scalar(x)}, y{Scalar(y)} + {} + + template + Vec2(Vec2 const &other) noexcept(std::is_nothrow_constructible_v) : Vec2(other.x, other.y) + {} + + static constexpr Vec2 + Zero() + { + return Vec2(0.f, 0.f); + } +}; + +template 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 diff --git a/src/xrt/tracking/hand/mercury/kine_lm/lm_interface.hpp b/src/xrt/tracking/hand/mercury/kine_lm/lm_interface.hpp new file mode 100644 index 000000000..d61a9b8bf --- /dev/null +++ b/src/xrt/tracking/hand/mercury/kine_lm/lm_interface.hpp @@ -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 + * @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 diff --git a/src/xrt/tracking/hand/mercury/kine_lm/lm_main.cpp b/src/xrt/tracking/hand/mercury/kine_lm/lm_main.cpp new file mode 100644 index 000000000..1e8c55520 --- /dev/null +++ b/src/xrt/tracking/hand/mercury/kine_lm/lm_main.cpp @@ -0,0 +1,992 @@ +// Copyright 2022, Collabora, Ltd. +// SPDX-License-Identifier: BSL-1.0 +/*! + * @file + * @brief Levenberg-Marquardt kinematic optimizer + * @author Moses Turner + * @author Charlton Rodda + * @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 +#include +#include +#include "lm_interface.hpp" +#include "lm_optimizer_params_packer.inl" +#include "lm_defines.hpp" + +/* + +Some notes: +Everything templated with is basically just a scalar template, usually taking float or ceres::Jet + +*/ + +namespace xrt::tracking::hand::mercury::lm { + +template struct StereographicObservation +{ + // T obs[kNumNNJoints][2]; + Vec2 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 sgo[2]; + + Quat last_frame_pre_rotation; + OptimizerHand 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 left_in_right_translation; + + // The orientation part of the same pose, just easier for Ceres to consume + Quat left_in_right_orientation; + + Eigen::Matrix TinyOptimizerInput; +}; + +template struct Translations55 +{ + Vec3 t[kNumFingers][kNumJointsInFinger]; +}; + +template struct Orientations54 +{ + Quat q[kNumFingers][kNumJointsInFinger]; +}; + +template struct CostFunctor +{ + KinematicHandLM &parent; + size_t num_residuals_; + + template + 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 +static inline void +eval_hand_set_rel_translations(const OptimizerHand &opt, Translations55 &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 +inline void +eval_hand_set_rel_orientations(const OptimizerHand &opt, Orientations54 &rel_orientations) +{ + +// Thumb MCP hidden orientation +#if 0 + Vec2 mcp_root_swing; + + mcp_root_swing.x = rad((T)(-10)); + mcp_root_swing.y = rad((T)(-40)); + + T mcp_root_twist = rad((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 +void +eval_hand_with_orientation(const OptimizerHand &opt, + bool is_right, + Translations55 &translations_absolute, + Orientations54 &orientations_absolute) + +{ + XRT_TRACE_MARKER(); + + + Translations55 rel_translations; //[kNumFingers][kNumJointsInFinger]; + Orientations54 rel_orientations; //[kNumFingers][kNumOrientationsInFinger]; + + eval_hand_set_rel_orientations(opt, rel_orientations); + + eval_hand_set_rel_translations(opt, rel_translations); + + Quat orientation_root; + + Quat 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 *last_orientation = &orientation_root; + for (size_t bone = 0; bone < kNumOrientationsInFinger; bone++) { + Quat &out_orientation = orientations_absolute.q[finger][bone]; + Quat &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 *last_translation = &opt.wrist_location; + const Quat *last_orientation = &orientation_root; + for (size_t bone = 0; bone < kNumJointsInFinger; bone++) { + Vec3 &out_translation = translations_absolute.t[finger][bone]; + Vec3 &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 +void +computeResidualStability_Finger(const OptimizerFinger &finger, + const OptimizerFinger &finger_last, + ResidualHelper &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 +void +computeResidualStability(const OptimizerHand &hand, + const OptimizerHand &last_hand, + KinematicHandLM &state, + ResidualHelper &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 &finger_last = last_hand.finger[finger_idx]; + + const OptimizerFinger &finger = hand.finger[finger_idx]; + + computeResidualStability_Finger(finger, finger_last, helper); + } +} + +template +inline void +normalize_vector_inplace(Vec3 &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 +static inline void +unit_vector_stereographic_projection(const Vec3 &in, Vec2 &out) +{ + out.x = in.x / ((T)1 - in.z); + out.y = in.y / ((T)1 - in.z); +} + + +template +static inline void +unit_xrt_vec3_stereographic_projection(const xrt_vec3 in, Vec2 &out) +{ + Vec3 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 +static void +diff(const Vec3 &model_joint_pos, + const Vec3 &move_joint_translation, + const Quat &move_joint_orientation, + const StereographicObservation &observation, + const HandScalar *confidences, + const HandScalar amount_we_care, + int &hand_joint_idx, + ResidualHelper &helper) +{ + + Vec3 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 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 &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 +void +CostFunctor_PositionsPart(OptimizerHand &hand, KinematicHandLM &state, ResidualHelper &helper) +{ + + Translations55 translations_absolute; + Orientations54 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 move_direction; + Quat move_orientation; + + if (view == 0) { + move_direction = Vec3::Zero(); + move_orientation = Quat::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(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(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 +// void CostFunctor_HandSizePart(OptimizerHand &hand, KinematicHandLM &state, T *residual, size_t &out_residual_idx) +// { + +// } + +template +template +bool +CostFunctor::operator()(const T *const x, T *residual) const +{ + struct KinematicHandLM &state = this->parent; + OptimizerHand hand = {}; + // ??? should I do the below? probably. + Quat tmp = this->parent.last_frame_pre_rotation; + OptimizerHandInit(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 helper(residual); + + CostFunctor_PositionsPart(hand, state, helper); + computeResidualStability(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 +static inline void +zldtt_ori_right(Quat &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 +static inline void +zldtt_ori_left(Quat &orientation, xrt_quat *out) +{ + out->w = orientation.w; + out->x = orientation.x; + out->y = orientation.y; + out->z = orientation.z; +} + +template +static inline void +zldtt(Vec3 &trans, Quat &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 pose = state.TinyOptimizerInput.cast(); + + OptimizerHand opt = {}; + OptimizerHandInit(opt, state.last_frame_pre_rotation); + OptimizerHandUnpackFromVector(pose.data(), state.optimize_hand_size, state.target_hand_size, opt); + + Translations55 translations_absolute; + Orientations54 orientations_absolute; + // Vec3 translations_absolute[kNumFingers][kNumJointsInFinger]; + // Quat orientations_absolute[kNumFingers][kNumOrientationsInFinger]; + + eval_hand_with_orientation(opt, state.is_right, translations_absolute, orientations_absolute); + + Quat post_wrist_orientation; + + AngleAxisToQuaternion(opt.wrist_post_orientation_aax, post_wrist_orientation); + + Quat 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 final_wrist_orientation; + + QuaternionProduct(pre_wrist_orientation, post_wrist_orientation, final_wrist_orientation); + + int joint_acc_idx = 0; + + // Palm. + + Vec3 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 &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 *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 +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 cf(state, residual_size); + + using AutoDiffCostFunctor = + ceres::TinySolverAutoDiffFunction, 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 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 inp = state.TinyOptimizerInput.head(); + + 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() = 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(state, observation, out_viz_hand); + } else { + opt_run(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 diff --git a/src/xrt/tracking/hand/mercury/kine_lm/lm_optimizer_params_packer.inl b/src/xrt/tracking/hand/mercury/kine_lm/lm_optimizer_params_packer.inl new file mode 100644 index 000000000..a3b74a6c3 --- /dev/null +++ b/src/xrt/tracking/hand/mercury/kine_lm/lm_optimizer_params_packer.inl @@ -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 + * @author Charlton Rodda + * @ingroup tracking + */ + +#include "util/u_logging.h" +#include "math/m_api.h" + +#include "lm_interface.hpp" +#include "lm_defines.hpp" +#include +#include + +// #include "lm_rotations.hpp" + +namespace xrt::tracking::hand::mercury::lm { + +template struct OptimizerMetacarpalBone +{ + Vec2 swing; + T twist; +}; + +template struct OptimizerFinger +{ + OptimizerMetacarpalBone metacarpal; + Vec2 proximal_swing; + // Not Vec2. + T rots[2]; +}; + +template struct OptimizerThumb +{ + OptimizerMetacarpalBone metacarpal; + // Again not Vec2. + T rots[2]; +}; + +template struct OptimizerHand +{ + T hand_size; + Vec3 wrist_location; + // This is constant, a ceres::Rotation.h quat,, taken from last frame. + Quat 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 wrist_post_orientation_aax; + OptimizerThumb thumb = {}; + OptimizerFinger 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(-60), rad(60)}; + thumb_mcp_swing_y = {rad(-60), rad(60)}; + thumb_mcp_twist = {rad(-35), rad(35)}; + + for (int i = 0; i < 2; i++) { + thumb_curls[i] = {rad(-90), rad(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(-10), rad(10)}; + finger.mcp_twist = {rad(-4), rad(4)}; + + finger.pxm_swing_x = {rad(-100), rad(20)}; // ??? why is it reversed + finger.pxm_swing_y = {rad(-20), rad(20)}; + + for (int i = 0; i < 2; i++) { + finger.curls[i] = {rad(-90), rad(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 +inline T +LMToModel(T lm, minmax mm) +{ + return mm.min + ((sin(lm) + T(1)) * ((mm.max - mm.min) * T(.5))); +} + +template +inline T +ModelToLM(T model, minmax mm) +{ + return asin((2 * (model - mm.min) / (mm.max - mm.min)) - 1); +} + +// Input vector, +template +void +OptimizerHandUnpackFromVector(const T *in, bool use_hand_size, T hand_size, OptimizerHand &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 +void +OptimizerHandPackIntoVector(OptimizerHand &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 +void +OptimizerHandInit(OptimizerHand &opt, Quat &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)(15)); + opt.finger[i].rots[0] = rad((T)(-5)); + opt.finger[i].rots[1] = rad((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)(-5)); + opt.thumb.rots[1] = rad((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 +void +OptimizerHandSquashRotations(OptimizerHand &opt, Quat &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 &pre_rotation = opt.wrist_pre_orientation_quat; + + Quat post_rotation; + + AngleAxisToQuaternion(opt.wrist_post_orientation_aax, post_rotation); + + Quat 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 diff --git a/src/xrt/tracking/hand/mercury/kine_lm/lm_rotations.inl b/src/xrt/tracking/hand/mercury/kine_lm/lm_rotations.inl new file mode 100644 index 000000000..3ad808063 --- /dev/null +++ b/src/xrt/tracking/hand/mercury/kine_lm/lm_rotations.inl @@ -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 + * @author Sameer Agarwal + * @author Moses Turner + * @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 +#include +#include +#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 +#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 +inline void +QuaternionProduct(const Quat &z, const Quat &w, Quat &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 +inline void +UnitQuaternionRotatePoint(const Quat &q, const Vec3 &pt, Vec3 &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 +inline void +UnitQuaternionRotateAndScalePoint(const Quat &q, const Vec3 &pt, const T scale, Vec3 &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 +inline void +AngleAxisToQuaternion(const Vec3 angle_axis, Quat &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 +inline void +CurlToQuaternion(const T &curl, Quat &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 +inline void +SwingToQuaternion(const Vec2 swing, Quat &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 +inline void +SwingTwistToQuaternion(const Vec2 swing, const T twist, Quat &result) +{ + //!@todo + // Rather than doing compound operations, we should derive it and collapse them. + Quat swing_quat; + Quat twist_quat; + + Vec3 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