The open source OpenXR runtime
h/mercury: Add Levenberg-Marquardt optimizer, and lots of fixes!
Co-authored-by: Charlton Rodda <charlton.rodda@collabora.com>
Co-authored-by: Ryan Pavlik <ryan.pavlik@collabora.com>
+3397
-618
+57
-8
src/xrt/tracking/hand/mercury/CMakeLists.txt
+57
-8
src/xrt/tracking/hand/mercury/CMakeLists.txt
···
1
1
# Copyright 2019-2022, Collabora, Ltd.
2
2
# SPDX-License-Identifier: BSL-1.0
3
3
4
+
4
5
# Mercury hand tracking library!
5
6
6
-
add_library(t_ht_mercury_kine STATIC kine/kinematic_interface.hpp kine/kinematic_main.cpp)
7
+
add_subdirectory(kine_lm)
8
+
add_subdirectory(kine_ccdik)
9
+
10
+
add_library(
11
+
t_ht_mercury_model STATIC
12
+
hg_model.cpp
13
+
)
14
+
15
+
16
+
target_link_libraries(
17
+
t_ht_mercury_model
18
+
PRIVATE
19
+
aux_math
20
+
aux_tracking
21
+
aux_os
22
+
aux_util
23
+
)
24
+
25
+
target_include_directories(t_ht_mercury_model
26
+
SYSTEM PRIVATE
27
+
${OpenCV_INCLUDE_DIRS}
28
+
${EIGEN3_INCLUDE_DIR}
29
+
)
30
+
31
+
target_link_libraries(
32
+
t_ht_mercury_model
33
+
PRIVATE
34
+
aux_math
35
+
aux_tracking
36
+
aux_os
37
+
aux_util
38
+
${OpenCV_LIBRARIES}
39
+
ONNXRuntime::ONNXRuntime
40
+
)
7
41
8
-
target_link_libraries(t_ht_mercury_kine PRIVATE aux_math aux_tracking aux_os aux_util)
42
+
add_library(
43
+
t_ht_mercury STATIC
44
+
hg_sync.cpp
45
+
hg_sync.hpp
46
+
hg_interface.h
47
+
kine_common.hpp
48
+
)
9
49
10
-
target_include_directories(t_ht_mercury_kine SYSTEM PRIVATE ${EIGEN3_INCLUDE_DIR})
11
50
12
-
add_library(t_ht_mercury STATIC hg_interface.h hg_model.hpp hg_sync.cpp hg_sync.hpp)
13
51
target_link_libraries(
14
52
t_ht_mercury
15
53
PUBLIC aux-includes xrt-external-cjson
···
19
57
aux_os
20
58
aux_util
21
59
ONNXRuntime::ONNXRuntime
22
-
t_ht_mercury_kine
60
+
t_ht_mercury_kine_lm
61
+
t_ht_mercury_kine_ccdik
62
+
t_ht_mercury_model
23
63
# ncnn # Soon...
24
64
${OpenCV_LIBRARIES}
25
65
)
66
+
26
67
if(XRT_HAVE_OPENCV)
27
-
target_include_directories(
28
-
t_ht_mercury SYSTEM PRIVATE ${OpenCV_INCLUDE_DIRS} ${EIGEN3_INCLUDE_DIR}
29
-
)
68
+
target_include_directories(t_ht_mercury SYSTEM PRIVATE ${OpenCV_INCLUDE_DIRS} ${EIGEN3_INCLUDE_DIR})
30
69
target_link_libraries(t_ht_mercury PUBLIC ${OpenCV_LIBRARIES})
31
70
endif()
71
+
72
+
73
+
# Below is entirely just so that tests can find us.
74
+
add_library(
75
+
t_ht_mercury_includes INTERFACE
76
+
)
77
+
78
+
target_include_directories(
79
+
t_ht_mercury_includes INTERFACE ${CMAKE_CURRENT_SOURCE_DIR} ${CMAKE_CURRENT_BINARY_DIR}
80
+
)
-174
src/xrt/tracking/hand/mercury/hg_image_math.hpp
-174
src/xrt/tracking/hand/mercury/hg_image_math.hpp
···
1
-
// Copyright 2021-2022, Collabora, Ltd.
2
-
// SPDX-License-Identifier: BSL-1.0
3
-
/*!
4
-
* @file
5
-
* @brief Helper header for drawing and image transforms
6
-
* @author Moses Turner <moses@collabora.com>
7
-
* @ingroup tracking
8
-
*/
9
-
#pragma once
10
-
11
-
#include "hg_sync.hpp"
12
-
13
-
namespace xrt::tracking::hand::mercury {
14
-
15
-
/*!
16
-
* This is a template so that we can use xrt_vec3 or xrt_vec2.
17
-
* Please don't use this for anything other than xrt_vec3 or xrt_vec2!
18
-
*/
19
-
template <typename T>
20
-
T
21
-
transformVecBy2x3(T in, cv::Matx23f warp_back)
22
-
{
23
-
T rr;
24
-
rr.x = (in.x * warp_back(0, 0)) + (in.y * warp_back(0, 1)) + warp_back(0, 2);
25
-
rr.y = (in.x * warp_back(1, 0)) + (in.y * warp_back(1, 1)) + warp_back(1, 2);
26
-
return rr;
27
-
}
28
-
29
-
cv::Scalar
30
-
hsv2rgb(float fH, float fS, float fV)
31
-
{
32
-
const float fC = fV * fS; // Chroma
33
-
const float fHPrime = fmod(fH / 60.0, 6);
34
-
const float fX = fC * (1 - fabs(fmod(fHPrime, 2) - 1));
35
-
const float fM = fV - fC;
36
-
37
-
float fR, fG, fB;
38
-
39
-
if (0 <= fHPrime && fHPrime < 1) {
40
-
fR = fC;
41
-
fG = fX;
42
-
fB = 0;
43
-
} else if (1 <= fHPrime && fHPrime < 2) {
44
-
fR = fX;
45
-
fG = fC;
46
-
fB = 0;
47
-
} else if (2 <= fHPrime && fHPrime < 3) {
48
-
fR = 0;
49
-
fG = fC;
50
-
fB = fX;
51
-
} else if (3 <= fHPrime && fHPrime < 4) {
52
-
fR = 0;
53
-
fG = fX;
54
-
fB = fC;
55
-
} else if (4 <= fHPrime && fHPrime < 5) {
56
-
fR = fX;
57
-
fG = 0;
58
-
fB = fC;
59
-
} else if (5 <= fHPrime && fHPrime < 6) {
60
-
fR = fC;
61
-
fG = 0;
62
-
fB = fX;
63
-
} else {
64
-
fR = 0;
65
-
fG = 0;
66
-
fB = 0;
67
-
}
68
-
69
-
fR += fM;
70
-
fG += fM;
71
-
fB += fM;
72
-
return {fR * 255.0f, fG * 255.0f, fB * 255.0f};
73
-
}
74
-
75
-
struct xrt_vec2
76
-
raycoord(ht_view *htv, struct xrt_vec2 model_out)
77
-
{
78
-
model_out.x *= htv->hgt->multiply_px_coord_for_undistort;
79
-
model_out.y *= htv->hgt->multiply_px_coord_for_undistort;
80
-
cv::Mat in_px_coords(1, 1, CV_32FC2);
81
-
float *write_in;
82
-
write_in = in_px_coords.ptr<float>(0);
83
-
write_in[0] = model_out.x;
84
-
write_in[1] = model_out.y;
85
-
cv::Mat out_ray(1, 1, CV_32FC2);
86
-
87
-
if (htv->hgt->use_fisheye) {
88
-
cv::fisheye::undistortPoints(in_px_coords, out_ray, htv->cameraMatrix, htv->distortion);
89
-
} else {
90
-
cv::undistortPoints(in_px_coords, out_ray, htv->cameraMatrix, htv->distortion);
91
-
}
92
-
93
-
float n_x = out_ray.at<float>(0, 0);
94
-
float n_y = out_ray.at<float>(0, 1);
95
-
96
-
97
-
struct xrt_vec3 n = {n_x, n_y, 1.0f};
98
-
99
-
cv::Matx33f R = htv->rotate_camera_to_stereo_camera;
100
-
101
-
struct xrt_vec3 o = {
102
-
(n.x * R(0, 0)) + (n.y * R(0, 1)) + (n.z * R(0, 2)),
103
-
(n.x * R(1, 0)) + (n.y * R(1, 1)) + (n.z * R(1, 2)),
104
-
(n.x * R(2, 0)) + (n.y * R(2, 1)) + (n.z * R(2, 2)),
105
-
};
106
-
107
-
math_vec3_scalar_mul(1.0f / o.z, &o);
108
-
return {o.x, o.y};
109
-
}
110
-
111
-
cv::Matx23f
112
-
blackbar(const cv::Mat &in, cv::Mat &out, xrt_size out_size)
113
-
{
114
-
#if 1
115
-
// Easy to think about, always right, but pretty slow:
116
-
// Get a matrix from the original to the scaled down / blackbar'd image, then get one that goes back.
117
-
// Then just warpAffine() it.
118
-
// Easy in programmer time - never have to worry about off by one, special cases. We can come back and optimize
119
-
// later.
120
-
121
-
// Do the black bars need to be on top and bottom, or on left and right?
122
-
float scale_down_w = (float)out_size.w / (float)in.cols; // 128/1280 = 0.1
123
-
float scale_down_h = (float)out_size.h / (float)in.rows; // 128/800 = 0.16
124
-
125
-
float scale_down = fmin(scale_down_w, scale_down_h); // 0.1
126
-
127
-
float width_inside = (float)in.cols * scale_down;
128
-
float height_inside = (float)in.rows * scale_down;
129
-
130
-
float translate_x = (out_size.w - width_inside) / 2; // should be 0 for 1280x800
131
-
float translate_y = (out_size.h - height_inside) / 2; // should be (1280-800)/2 = 240
132
-
133
-
cv::Matx23f go;
134
-
// clang-format off
135
-
go(0,0) = scale_down; go(0,1) = 0.0f; go(0,2) = translate_x;
136
-
go(1,0) = 0.0f; go(1,1) = scale_down; go(1,2) = translate_y;
137
-
// clang-format on
138
-
139
-
cv::warpAffine(in, out, go, cv::Size(out_size.w, out_size.h));
140
-
141
-
cv::Matx23f ret;
142
-
143
-
// clang-format off
144
-
ret(0,0) = 1.0f/scale_down; ret(0,1) = 0.0f; ret(0,2) = -translate_x/scale_down;
145
-
ret(1,0) = 0.0f; ret(1,1) = 1.0f/scale_down; ret(1,2) = -translate_y/scale_down;
146
-
// clang-format on
147
-
148
-
return ret;
149
-
#else
150
-
// Fast, always wrong if the input isn't square. You'd end up using something like this, plus some
151
-
// copyMakeBorder if you want to optimize.
152
-
if (aspect_ratio_input == aspect_ratio_output) {
153
-
cv::resize(in, out, {out_size.w, out_size.h});
154
-
cv::Matx23f ret;
155
-
float scale_from_out_to_in = (float)in.cols / (float)out_size.w;
156
-
// clang-format off
157
-
ret(0,0) = scale_from_out_to_in; ret(0,1) = 0.0f; ret(0,2) = 0.0f;
158
-
ret(1,0) = 0.0f; ret(1,1) = scale_from_out_to_in; ret(1,2) = 0.0f;
159
-
// clang-format on
160
-
cv::imshow("hi", out);
161
-
cv::waitKey(1);
162
-
return ret;
163
-
}
164
-
assert(!"Uh oh! Unimplemented!");
165
-
return {};
166
-
#endif
167
-
}
168
-
169
-
void
170
-
handDot(cv::Mat &mat, xrt_vec2 place, float radius, float hue, float intensity, int type)
171
-
{
172
-
cv::circle(mat, {(int)place.x, (int)place.y}, radius, hsv2rgb(hue * 360.0f, intensity, intensity), type);
173
-
}
174
-
} // namespace xrt::tracking::hand::mercury
+109
src/xrt/tracking/hand/mercury/hg_image_math.inl
+109
src/xrt/tracking/hand/mercury/hg_image_math.inl
···
1
+
// Copyright 2021-2022, Collabora, Ltd.
2
+
// SPDX-License-Identifier: BSL-1.0
3
+
/*!
4
+
* @file
5
+
* @brief Helper header for drawing and image transforms
6
+
* @author Moses Turner <moses@collabora.com>
7
+
* @ingroup tracking
8
+
*/
9
+
#pragma once
10
+
11
+
#include "hg_sync.hpp"
12
+
13
+
namespace xrt::tracking::hand::mercury {
14
+
15
+
/*!
16
+
* This is a template so that we can use xrt_vec3 or xrt_vec2.
17
+
* Please don't use this for anything other than xrt_vec3 or xrt_vec2!
18
+
*/
19
+
template <typename T>
20
+
T
21
+
transformVecBy2x3(T in, cv::Matx23f warp_back)
22
+
{
23
+
T rr;
24
+
rr.x = (in.x * warp_back(0, 0)) + (in.y * warp_back(0, 1)) + warp_back(0, 2);
25
+
rr.y = (in.x * warp_back(1, 0)) + (in.y * warp_back(1, 1)) + warp_back(1, 2);
26
+
return rr;
27
+
}
28
+
29
+
static cv::Scalar
30
+
hsv2rgb(float fH, float fS, float fV)
31
+
{
32
+
const float fC = fV * fS; // Chroma
33
+
const float fHPrime = fmod(fH / 60.0, 6);
34
+
const float fX = fC * (1 - fabs(fmod(fHPrime, 2) - 1));
35
+
const float fM = fV - fC;
36
+
37
+
float fR, fG, fB;
38
+
39
+
if (0 <= fHPrime && fHPrime < 1) {
40
+
fR = fC;
41
+
fG = fX;
42
+
fB = 0;
43
+
} else if (1 <= fHPrime && fHPrime < 2) {
44
+
fR = fX;
45
+
fG = fC;
46
+
fB = 0;
47
+
} else if (2 <= fHPrime && fHPrime < 3) {
48
+
fR = 0;
49
+
fG = fC;
50
+
fB = fX;
51
+
} else if (3 <= fHPrime && fHPrime < 4) {
52
+
fR = 0;
53
+
fG = fX;
54
+
fB = fC;
55
+
} else if (4 <= fHPrime && fHPrime < 5) {
56
+
fR = fX;
57
+
fG = 0;
58
+
fB = fC;
59
+
} else if (5 <= fHPrime && fHPrime < 6) {
60
+
fR = fC;
61
+
fG = 0;
62
+
fB = fX;
63
+
} else {
64
+
fR = 0;
65
+
fG = 0;
66
+
fB = 0;
67
+
}
68
+
69
+
fR += fM;
70
+
fG += fM;
71
+
fB += fM;
72
+
return {fR * 255.0f, fG * 255.0f, fB * 255.0f};
73
+
}
74
+
75
+
76
+
//! @optimize Make it take an array of vec2's and give out an array of vec2's, then
77
+
// put it in its own target so we don't have to link to OpenCV.
78
+
// Oooorrrrr... actually add good undistortion stuff to Monado so that we don't have to depend on OpenCV at all.
79
+
80
+
XRT_MAYBE_UNUSED static struct xrt_vec2
81
+
raycoord(ht_view *htv, struct xrt_vec2 model_out)
82
+
{
83
+
model_out.x *= htv->hgt->multiply_px_coord_for_undistort;
84
+
model_out.y *= htv->hgt->multiply_px_coord_for_undistort;
85
+
cv::Mat in_px_coords(1, 1, CV_32FC2);
86
+
float *write_in;
87
+
write_in = in_px_coords.ptr<float>(0);
88
+
write_in[0] = model_out.x;
89
+
write_in[1] = model_out.y;
90
+
cv::Mat out_ray(1, 1, CV_32FC2);
91
+
92
+
if (htv->hgt->use_fisheye) {
93
+
cv::fisheye::undistortPoints(in_px_coords, out_ray, htv->cameraMatrix, htv->distortion);
94
+
} else {
95
+
cv::undistortPoints(in_px_coords, out_ray, htv->cameraMatrix, htv->distortion);
96
+
}
97
+
98
+
float n_x = out_ray.at<float>(0, 0);
99
+
float n_y = out_ray.at<float>(0, 1);
100
+
101
+
return {n_x, n_y};
102
+
}
103
+
104
+
static void
105
+
handDot(cv::Mat &mat, xrt_vec2 place, float radius, float hue, float intensity, int type)
106
+
{
107
+
cv::circle(mat, {(int)place.x, (int)place.y}, radius, hsv2rgb(hue * 360.0f, intensity, intensity), type);
108
+
}
109
+
} // namespace xrt::tracking::hand::mercury
+98
-35
src/xrt/tracking/hand/mercury/hg_model.hpp
src/xrt/tracking/hand/mercury/hg_model.cpp
+98
-35
src/xrt/tracking/hand/mercury/hg_model.hpp
src/xrt/tracking/hand/mercury/hg_model.cpp
···
9
9
10
10
// Many C api things were stolen from here (MIT license):
11
11
// https://github.com/microsoft/onnxruntime-inference-examples/blob/main/c_cxx/fns_candy_style_transfer/fns_candy_style_transfer.c
12
-
#pragma once
13
-
14
12
#include "hg_sync.hpp"
15
-
#include "hg_image_math.hpp"
13
+
#include "hg_image_math.inl"
16
14
17
15
18
16
#include <filesystem>
19
17
#include <array>
20
18
21
19
namespace xrt::tracking::hand::mercury {
22
-
23
-
cv::Scalar RED(255, 30, 30);
24
-
cv::Scalar YELLOW(255, 255, 0);
25
-
26
-
cv::Scalar colors[2] = {YELLOW, RED};
27
20
28
21
#define ORT(expr) \
29
22
do { \
···
37
30
} while (0)
38
31
39
32
40
-
static bool
41
-
argmax(const float *data, int size, int *out_idx)
33
+
static cv::Matx23f
34
+
blackbar(const cv::Mat &in, cv::Mat &out, xrt_size out_size)
35
+
{
36
+
#if 1
37
+
// Easy to think about, always right, but pretty slow:
38
+
// Get a matrix from the original to the scaled down / blackbar'd image, then get one that goes back.
39
+
// Then just warpAffine() it.
40
+
// Easy in programmer time - never have to worry about off by one, special cases. We can come back and optimize
41
+
// later.
42
+
43
+
// Do the black bars need to be on top and bottom, or on left and right?
44
+
float scale_down_w = (float)out_size.w / (float)in.cols; // 128/1280 = 0.1
45
+
float scale_down_h = (float)out_size.h / (float)in.rows; // 128/800 = 0.16
46
+
47
+
float scale_down = fmin(scale_down_w, scale_down_h); // 0.1
48
+
49
+
float width_inside = (float)in.cols * scale_down;
50
+
float height_inside = (float)in.rows * scale_down;
51
+
52
+
float translate_x = (out_size.w - width_inside) / 2; // should be 0 for 1280x800
53
+
float translate_y = (out_size.h - height_inside) / 2; // should be (1280-800)/2 = 240
54
+
55
+
cv::Matx23f go;
56
+
// clang-format off
57
+
go(0,0) = scale_down; go(0,1) = 0.0f; go(0,2) = translate_x;
58
+
go(1,0) = 0.0f; go(1,1) = scale_down; go(1,2) = translate_y;
59
+
// clang-format on
60
+
61
+
cv::warpAffine(in, out, go, cv::Size(out_size.w, out_size.h));
62
+
63
+
cv::Matx23f ret;
64
+
65
+
// clang-format off
66
+
ret(0,0) = 1.0f/scale_down; ret(0,1) = 0.0f; ret(0,2) = -translate_x/scale_down;
67
+
ret(1,0) = 0.0f; ret(1,1) = 1.0f/scale_down; ret(1,2) = -translate_y/scale_down;
68
+
// clang-format on
69
+
70
+
return ret;
71
+
#else
72
+
// Fast, always wrong if the input isn't square. You'd end up using something like this, plus some
73
+
// copyMakeBorder if you want to optimize.
74
+
if (aspect_ratio_input == aspect_ratio_output) {
75
+
cv::resize(in, out, {out_size.w, out_size.h});
76
+
cv::Matx23f ret;
77
+
float scale_from_out_to_in = (float)in.cols / (float)out_size.w;
78
+
// clang-format off
79
+
ret(0,0) = scale_from_out_to_in; ret(0,1) = 0.0f; ret(0,2) = 0.0f;
80
+
ret(1,0) = 0.0f; ret(1,1) = scale_from_out_to_in; ret(1,2) = 0.0f;
81
+
// clang-format on
82
+
cv::imshow("hi", out);
83
+
cv::waitKey(1);
84
+
return ret;
85
+
}
86
+
assert(!"Uh oh! Unimplemented!");
87
+
return {};
88
+
#endif
89
+
}
90
+
91
+
static inline int
92
+
argmax(const float *data, int size)
42
93
{
43
-
float max_value = -1.0f;
44
-
bool found = false;
45
-
for (int i = 0; i < size; i++) {
94
+
float max_value = data[0];
95
+
int out_idx = 0;
96
+
97
+
for (int i = 1; i < size; i++) {
46
98
if (data[i] > max_value) {
47
99
max_value = data[i];
48
-
*out_idx = i;
49
-
found = true;
100
+
out_idx = i;
50
101
}
51
102
}
52
-
return found;
103
+
return out_idx;
53
104
}
54
105
55
-
void
106
+
static void
56
107
refine_center_of_distribution(
57
108
const float *data, int coarse_x, int coarse_y, int w, int h, float *out_refined_x, float *out_refined_y)
58
109
{
···
139
190
cv::Mat stddev;
140
191
cv::meanStdDev(data_out, mean, stddev);
141
192
193
+
if (stddev.at<double>(0, 0) == 0) {
194
+
U_LOG_W("Got image with zero standard deviation!");
195
+
return;
196
+
}
197
+
142
198
data_out *= 0.25 / stddev.at<double>(0, 0);
143
199
144
200
// Calculate it again; mean has changed. Yes we don't need to but it's easy
···
147
203
data_out += (0.5 - mean.at<double>(0, 0));
148
204
}
149
205
150
-
static void
206
+
void
151
207
init_hand_detection(HandTracking *hgt, onnx_wrap *wrap)
152
208
{
153
209
std::filesystem::path path = hgt->models_folder;
···
203
259
{
204
260
XRT_TRACE_MARKER();
205
261
206
-
ht_view *view = (ht_view *)ptr;
262
+
// ht_view *view = (ht_view *)ptr;
263
+
264
+
hand_detection_run_info *info = (hand_detection_run_info *)ptr;
265
+
ht_view *view = info->view;
266
+
207
267
HandTracking *hgt = view->hgt;
208
268
onnx_wrap *wrap = &view->detection;
209
269
cv::Mat &data_400x640 = view->run_model_on_this;
270
+
210
271
211
272
cv::Mat _240x320_uint8;
212
273
···
234
295
235
296
for (int hand_idx = 0; hand_idx < 2; hand_idx++) {
236
297
const float *this_side_data = out_data + hand_idx * plane_size * 2;
237
-
int max_idx;
298
+
int max_idx = argmax(this_side_data, 4800);
238
299
239
-
det_output *output = &view->det_outputs[hand_idx];
300
+
hand_bounding_box *output = info->outputs[hand_idx];
240
301
241
-
output->found = argmax(this_side_data, 4800, &max_idx) && this_side_data[max_idx] > 0.3;
302
+
output->found = this_side_data[max_idx] > 0.3;
242
303
243
304
if (output->found) {
305
+
output->confidence = this_side_data[max_idx];
244
306
245
307
int row = max_idx / 80;
246
308
int col = max_idx % 80;
···
268
330
cv::Point2i pt(_pt.x, _pt.y);
269
331
cv::rectangle(debug_frame,
270
332
cv::Rect(pt - cv::Point2i(size / 2, size / 2), cv::Size(size, size)),
271
-
colors[hand_idx], 1);
333
+
PINK, 1);
272
334
}
273
335
}
274
336
}
···
276
338
wrap->api->ReleaseValue(output_tensor);
277
339
}
278
340
279
-
static void
341
+
void
280
342
init_keypoint_estimation(HandTracking *hgt, onnx_wrap *wrap)
281
343
{
282
344
···
341
403
342
404
cv::Mat &debug = info->view->debug_out_to_this;
343
405
344
-
det_output *output = &info->view->det_outputs[info->hand_idx];
406
+
hand_bounding_box *output = &info->view->bboxes_this_frame[info->hand_idx];
345
407
346
408
cv::Point2f src_tri[3];
347
409
cv::Point2f dst_tri[3];
···
402
464
cv::Mat y(cv::Size(128, 21), CV_32FC1, out_data_y);
403
465
404
466
for (int i = 0; i < 21; i++) {
405
-
int loc_x;
406
-
int loc_y;
407
-
argmax(&out_data_x[i * 128], 128, &loc_x);
408
-
argmax(&out_data_y[i * 128], 128, &loc_y);
467
+
int loc_x = argmax(&out_data_x[i * 128], 128);
468
+
int loc_y = argmax(&out_data_y[i * 128], 128);
409
469
xrt_vec2 loc;
410
470
loc.x = loc_x;
411
471
loc.y = loc_y;
···
415
475
tan_space.kps[i] = raycoord(info->view, loc);
416
476
}
417
477
418
-
if (hgt->debug_scribble) {
478
+
if (hgt->debug_scribble && hgt->tuneable_values.scribble_keypoint_model_outputs) {
419
479
for (int finger = 0; finger < 5; finger++) {
420
480
cv::Point last = {(int)keypoints_global[0].x, (int)keypoints_global[0].y};
421
481
for (int joint = 0; joint < 4; joint++) {
···
438
498
}
439
499
440
500
441
-
static void
501
+
void
442
502
init_keypoint_estimation_new(HandTracking *hgt, onnx_wrap *wrap)
443
503
{
444
504
···
508
568
509
569
cv::Mat &debug = info->view->debug_out_to_this;
510
570
511
-
det_output *output = &info->view->det_outputs[info->hand_idx];
571
+
hand_bounding_box *output = &info->view->bboxes_this_frame[info->hand_idx];
512
572
513
573
cv::Point2f src_tri[3];
514
574
cv::Point2f dst_tri[3];
···
570
630
571
631
Hand2D &px_coord = info->view->keypoint_outputs[info->hand_idx].hand_px_coord;
572
632
Hand2D &tan_space = info->view->keypoint_outputs[info->hand_idx].hand_tan_space;
633
+
float *confidences = info->view->keypoint_outputs[info->hand_idx].hand_tan_space.confidences;
573
634
xrt_vec2 *keypoints_global = px_coord.kps;
574
635
575
636
size_t plane_size = 22 * 22;
576
637
577
638
for (int i = 0; i < 21; i++) {
578
639
float *data = &out_data[i * plane_size];
579
-
int out_idx = 0;
580
-
argmax(data, 22 * 22, &out_idx);
640
+
int out_idx = argmax(data, 22 * 22);
581
641
int row = out_idx / 22;
582
642
int col = out_idx % 22;
583
643
584
644
xrt_vec2 loc;
645
+
585
646
586
647
refine_center_of_distribution(data, col, row, 22, 22, &loc.x, &loc.y);
587
648
···
589
650
loc.y *= 128.0f / 22.0f;
590
651
591
652
loc = transformVecBy2x3(loc, go_back);
653
+
654
+
confidences[i] = data[out_idx];
592
655
px_coord.kps[i] = loc;
593
656
594
657
tan_space.kps[i] = raycoord(info->view, loc);
595
658
}
596
659
597
-
if (hgt->debug_scribble) {
660
+
if (hgt->debug_scribble && hgt->tuneable_values.scribble_keypoint_model_outputs) {
598
661
for (int finger = 0; finger < 5; finger++) {
599
662
cv::Point last = {(int)keypoints_global[0].x, (int)keypoints_global[0].y};
600
663
for (int joint = 0; joint < 4; joint++) {
+705
-134
src/xrt/tracking/hand/mercury/hg_sync.cpp
+705
-134
src/xrt/tracking/hand/mercury/hg_sync.cpp
···
4
4
* @file
5
5
* @brief Mercury hand tracking main file.
6
6
* @author Jakob Bornecrantz <jakob@collabora.com>
7
-
* @author Moses Turner <jakob@collabora.com>
7
+
* @author Moses Turner <moses@collabora.com>
8
+
* @author Charlton Rodda <charlton.rodda@collabora.com>
8
9
* @author Nick Klingensmith <programmerpichu@gmail.com>
9
10
* @ingroup tracking
10
11
*/
11
12
12
13
#include "hg_sync.hpp"
13
-
#include "hg_model.hpp"
14
-
#include "util/u_sink.h"
14
+
#include "hg_image_math.inl"
15
+
#include "math/m_vec2.h"
16
+
#include "util/u_misc.h"
17
+
18
+
15
19
#include <numeric>
20
+
16
21
17
22
namespace xrt::tracking::hand::mercury {
23
+
#define DEG_TO_RAD(DEG) (DEG * M_PI / 180.)
24
+
25
+
DEBUG_GET_ONCE_LOG_OPTION(mercury_log, "MERCURY_LOG", U_LOGGING_WARN)
26
+
DEBUG_GET_ONCE_BOOL_OPTION(mercury_use_simdr_keypoint, "MERCURY_USE_SIMDR_KEYPOINT", false)
27
+
18
28
// Flags to tell state tracker that these are indeed valid joints
19
29
static const enum xrt_space_relation_flags valid_flags_ht = (enum xrt_space_relation_flags)(
20
30
XRT_SPACE_RELATION_ORIENTATION_VALID_BIT | XRT_SPACE_RELATION_ORIENTATION_TRACKED_BIT |
21
31
XRT_SPACE_RELATION_POSITION_VALID_BIT | XRT_SPACE_RELATION_POSITION_TRACKED_BIT);
22
32
23
-
static void
24
-
hgJointDisparityMath(struct HandTracking *hgt, Hand2D *hand_in_left, Hand2D *hand_in_right, Hand3D *out_hand)
25
-
{
26
-
for (int i = 0; i < 21; i++) {
27
-
// Believe it or not, this is where the 3D stuff happens!
28
-
float t = hgt->baseline / (hand_in_left->kps[i].x - hand_in_right->kps[i].x);
29
-
30
-
out_hand->kps[i].z = -t;
31
-
32
-
out_hand->kps[i].x = (hand_in_left->kps[i].x * t);
33
-
out_hand->kps[i].y = -hand_in_left->kps[i].y * t;
34
-
35
-
out_hand->kps[i].x += hgt->baseline + (hand_in_right->kps[i].x * t);
36
-
out_hand->kps[i].y += -hand_in_right->kps[i].y * t;
37
-
38
-
out_hand->kps[i].x *= .5;
39
-
out_hand->kps[i].y *= .5;
40
-
}
41
-
}
42
-
43
33
/*!
44
34
* Setup helper functions.
45
35
*/
46
36
47
-
static bool
48
-
getCalibration(struct HandTracking *hgt, t_stereo_camera_calibration *calibration)
37
+
38
+
//!@todo
39
+
// This is cruft, necessary because some users still use HT_OUTPUT_SPACE_CENTER_OF_STEREO_CAMERA.
40
+
// WMR especially should *not* be using it, and for custom North Star/
41
+
static void
42
+
get_left_camera_to_center(struct HandTracking *hgt, xrt::auxiliary::tracking::StereoCameraCalibrationWrapper &wrap)
49
43
{
50
-
xrt::auxiliary::tracking::StereoCameraCalibrationWrapper wrap(calibration);
51
-
xrt_vec3 trans = {(float)wrap.camera_translation_mat(0, 0), (float)wrap.camera_translation_mat(1, 0),
52
-
(float)wrap.camera_translation_mat(2, 0)};
53
-
hgt->baseline = m_vec3_len(trans);
54
-
HG_DEBUG(hgt, "I think the baseline is %f meters!", hgt->baseline);
55
-
// Note, this assumes camera 0 is the left camera and camera 1 is the right camera.
56
-
// If you find one with the opposite arrangement, you'll need to invert hgt->baseline, and look at
57
-
// hgJointDisparityMath
58
-
59
-
hgt->use_fisheye = wrap.view[0].use_fisheye;
60
-
61
-
if (hgt->use_fisheye) {
62
-
HG_DEBUG(hgt, "I think the cameras are fisheye!");
63
-
} else {
64
-
HG_DEBUG(hgt, "I think the cameras are not fisheye!");
65
-
}
44
+
cv::Matx33d R1;
45
+
cv::Matx33d R2;
66
46
67
47
cv::Matx34d P1;
68
48
cv::Matx34d P2;
···
105
85
NULL); // validPixROI2
106
86
}
107
87
88
+
// HG_DEBUG(hgt, "R%d ->", i);
89
+
// std::cout << hgt->views[i].rotate_camera_to_stereo_camera << std::endl;
90
+
91
+
92
+
// Untested, so far.
93
+
//!@todo Definitely this or getCalibration are doing a transpose and I don't know which.
94
+
cv::Matx33d rotate_stereo_camera_to_left_camera = hgt->views[0].rotate_camera_to_stereo_camera.inv();
95
+
96
+
xrt_matrix_3x3 s;
97
+
s.v[0] = rotate_stereo_camera_to_left_camera(0, 0);
98
+
s.v[1] = rotate_stereo_camera_to_left_camera(0, 1);
99
+
s.v[2] = rotate_stereo_camera_to_left_camera(0, 2);
100
+
101
+
s.v[3] = rotate_stereo_camera_to_left_camera(1, 0);
102
+
s.v[4] = rotate_stereo_camera_to_left_camera(1, 1);
103
+
s.v[5] = rotate_stereo_camera_to_left_camera(1, 2);
104
+
105
+
s.v[6] = rotate_stereo_camera_to_left_camera(2, 0);
106
+
s.v[7] = rotate_stereo_camera_to_left_camera(2, 1);
107
+
s.v[8] = rotate_stereo_camera_to_left_camera(2, 2);
108
+
109
+
110
+
math_quat_from_matrix_3x3(&s, &hgt->hand_pose_camera_offset.orientation);
111
+
hgt->hand_pose_camera_offset.position.x = -hgt->baseline / 2;
112
+
}
113
+
114
+
static bool
115
+
getCalibration(struct HandTracking *hgt, t_stereo_camera_calibration *calibration)
116
+
{
117
+
xrt::auxiliary::tracking::StereoCameraCalibrationWrapper wrap(calibration);
118
+
xrt_vec3 trans = {(float)wrap.camera_translation_mat(0, 0), (float)wrap.camera_translation_mat(1, 0),
119
+
(float)wrap.camera_translation_mat(2, 0)};
120
+
hgt->baseline = m_vec3_len(trans);
121
+
HG_DEBUG(hgt, "I think the baseline is %f meters!", hgt->baseline);
122
+
// Note, this assumes camera 0 is the left camera and camera 1 is the right camera.
123
+
// If you find one with the opposite arrangement, you'll need to invert hgt->baseline, and look at
124
+
// hgJointDisparityMath
125
+
126
+
hgt->use_fisheye = wrap.view[0].use_fisheye;
127
+
128
+
if (hgt->use_fisheye) {
129
+
HG_DEBUG(hgt, "I think the cameras are fisheye!");
130
+
} else {
131
+
HG_DEBUG(hgt, "I think the cameras are not fisheye!");
132
+
}
133
+
{
134
+
// Officially, I have no idea if this is row-major or col-major. Empirically it seems to work, and that
135
+
// is all I will say.
136
+
137
+
// It might be that the below is *transposing* the matrix, I never remember if OpenCV's R is "left in
138
+
// right" or "right in left"
139
+
//!@todo
140
+
xrt_matrix_3x3 s;
141
+
s.v[0] = wrap.camera_rotation_mat(0, 0);
142
+
s.v[1] = wrap.camera_rotation_mat(1, 0);
143
+
s.v[2] = wrap.camera_rotation_mat(2, 0);
144
+
145
+
s.v[3] = wrap.camera_rotation_mat(0, 1);
146
+
s.v[4] = wrap.camera_rotation_mat(1, 1);
147
+
s.v[5] = wrap.camera_rotation_mat(2, 1);
148
+
149
+
s.v[6] = wrap.camera_rotation_mat(0, 2);
150
+
s.v[7] = wrap.camera_rotation_mat(1, 2);
151
+
s.v[8] = wrap.camera_rotation_mat(2, 2);
152
+
153
+
xrt_pose left_in_right;
154
+
left_in_right.position.x = wrap.camera_translation_mat(0);
155
+
left_in_right.position.y = wrap.camera_translation_mat(1);
156
+
left_in_right.position.z = wrap.camera_translation_mat(2);
157
+
158
+
math_quat_from_matrix_3x3(&s, &left_in_right.orientation);
159
+
left_in_right.orientation.x = -left_in_right.orientation.x;
160
+
left_in_right.position.y = -left_in_right.position.y;
161
+
left_in_right.position.z = -left_in_right.position.z;
162
+
163
+
hgt->left_in_right = left_in_right;
164
+
165
+
U_LOG_E("OpenXR: %f %f %f %f %f %f %f", left_in_right.position.x, left_in_right.position.z,
166
+
left_in_right.position.z, left_in_right.orientation.x, left_in_right.orientation.y,
167
+
left_in_right.orientation.z, left_in_right.orientation.w);
168
+
}
169
+
170
+
171
+
108
172
//* Good enough guess that view 0 and view 1 are the same size.
109
173
for (int i = 0; i < 2; i++) {
110
174
hgt->views[i].cameraMatrix = wrap.view[i].intrinsics_mat;
···
116
180
}
117
181
118
182
if (hgt->log_level <= U_LOGGING_DEBUG) {
119
-
HG_DEBUG(hgt, "R%d ->", i);
120
-
std::cout << hgt->views[i].rotate_camera_to_stereo_camera << std::endl;
121
-
122
183
HG_DEBUG(hgt, "K%d ->", i);
123
184
std::cout << hgt->views[i].cameraMatrix << std::endl;
124
185
···
133
194
hgt->last_frame_one_view_size_px = hgt->calibration_one_view_size_px;
134
195
hgt->multiply_px_coord_for_undistort = 1.0f;
135
196
136
-
cv::Matx33d rotate_stereo_camera_to_left_camera = hgt->views[0].rotate_camera_to_stereo_camera.inv();
197
+
switch (hgt->output_space) {
198
+
case HT_OUTPUT_SPACE_LEFT_CAMERA: hgt->hand_pose_camera_offset = XRT_POSE_IDENTITY; break;
199
+
case HT_OUTPUT_SPACE_CENTER_OF_STEREO_CAMERA: get_left_camera_to_center(hgt, wrap); break;
200
+
}
137
201
138
-
xrt_matrix_3x3 s;
139
-
s.v[0] = rotate_stereo_camera_to_left_camera(0, 0);
140
-
s.v[1] = rotate_stereo_camera_to_left_camera(0, 1);
141
-
s.v[2] = rotate_stereo_camera_to_left_camera(0, 2);
142
202
143
-
s.v[3] = rotate_stereo_camera_to_left_camera(1, 0);
144
-
s.v[4] = rotate_stereo_camera_to_left_camera(1, 1);
145
-
s.v[5] = rotate_stereo_camera_to_left_camera(1, 2);
146
-
147
-
s.v[6] = rotate_stereo_camera_to_left_camera(2, 0);
148
-
s.v[7] = rotate_stereo_camera_to_left_camera(2, 1);
149
-
s.v[8] = rotate_stereo_camera_to_left_camera(2, 2);
150
-
151
-
xrt_quat tmp;
152
-
153
-
math_quat_from_matrix_3x3(&s, &tmp);
154
-
155
-
// Weird that I have to invert this quat, right? I think at some point - like probably just before this - I must
156
-
// have swapped row-major and col-major - remember, if you transpose a rotation matrix, you get its inverse.
157
-
// Doesn't matter that I don't understand - non-inverted looks definitely wrong, inverted looks definitely
158
-
// right.
159
-
math_quat_invert(&tmp, &hgt->stereo_camera_to_left_camera);
160
203
161
204
return true;
162
205
}
···
199
242
#endif
200
243
}
201
244
202
-
203
-
static void
204
-
applyThumbIndexDrag(Hand3D *hand)
205
-
{
206
-
// TERRIBLE HACK.
207
-
// Puts the thumb and pointer a bit closer together to be better at triggering XR clients' pinch detection.
208
-
static const float max_radius = 0.05;
209
-
static const float min_radius = 0.00;
210
-
211
-
// no min drag, min drag always 0.
212
-
static const float max_drag = 0.85f;
213
-
214
-
xrt_vec3 thumb = hand->kps[THMB_TIP];
215
-
xrt_vec3 index = hand->kps[INDX_TIP];
216
-
xrt_vec3 ttp = index - thumb;
217
-
float length = m_vec3_len(ttp);
218
-
if ((length > max_radius)) {
219
-
return;
220
-
}
221
-
222
-
223
-
float amount = math_map_ranges(length, min_radius, max_radius, max_drag, 0.0f);
224
-
225
-
hand->kps[THMB_TIP] = m_vec3_lerp(thumb, index, amount * 0.5f);
226
-
hand->kps[INDX_TIP] = m_vec3_lerp(index, thumb, amount * 0.5f);
227
-
}
228
-
229
245
static void
230
246
applyJointWidths(struct HandTracking *hgt, struct xrt_hand_joint_set *set)
231
247
{
···
258
274
.040f * .5f; // Measured my wrist thickness with calipers
259
275
}
260
276
277
+
template <typename Vec>
278
+
static inline bool
279
+
check_outside_view(struct HandTracking *hgt, struct hand_tracking_image_boundary_info_one_view boundary, Vec &keypoint)
280
+
{
281
+
// Regular case - the keypoint is literally outside the image
282
+
if (keypoint.y > hgt->calibration_one_view_size_px.h || //
283
+
keypoint.y < 0 || //
284
+
keypoint.x > hgt->calibration_one_view_size_px.w || //
285
+
keypoint.x < 0) {
286
+
return true;
287
+
}
288
+
289
+
switch (boundary.type) {
290
+
// No boundary, and we passed the previous check. Not outside the view.
291
+
case HT_IMAGE_BOUNDARY_NONE: return false; break;
292
+
case HT_IMAGE_BOUNDARY_CIRCLE: {
293
+
//!@optimize Most of this can be calculated once at startup
294
+
xrt_vec2 center_px = {
295
+
boundary.boundary.circle.normalized_center.x * (float)hgt->calibration_one_view_size_px.w, //
296
+
boundary.boundary.circle.normalized_center.y * (float)hgt->calibration_one_view_size_px.h};
297
+
float radius_px =
298
+
boundary.boundary.circle.normalized_radius * (float)hgt->calibration_one_view_size_px.w;
299
+
300
+
xrt_vec2 keypoint_xrt = {float(keypoint.x), float(keypoint.y)};
301
+
302
+
xrt_vec2 diff = center_px - keypoint_xrt;
303
+
if (m_vec2_len(diff) > radius_px) {
304
+
return true;
305
+
}
306
+
} break;
307
+
}
308
+
309
+
return false;
310
+
}
311
+
312
+
static void
313
+
back_project(struct HandTracking *hgt,
314
+
xrt_hand_joint_set *in,
315
+
bool also_debug_output,
316
+
xrt_vec2 centers[2],
317
+
float radii[2],
318
+
int num_outside[2])
319
+
{
320
+
321
+
for (int view_idx = 0; view_idx < 2; view_idx++) {
322
+
xrt_pose move_amount;
323
+
324
+
if (view_idx == 0) {
325
+
// left camera.
326
+
move_amount = XRT_POSE_IDENTITY;
327
+
} else {
328
+
move_amount = hgt->left_in_right;
329
+
}
330
+
std::vector<cv::Point3d> pts_relative_to_camera(26);
331
+
332
+
// Bandaid solution, doesn't quite fix things on WMR.
333
+
bool any_joint_behind_camera = false;
334
+
335
+
for (int i = 0; i < 26; i++) {
336
+
xrt_vec3 tmp;
337
+
math_quat_rotate_vec3(&move_amount.orientation,
338
+
&in->values.hand_joint_set_default[i].relation.pose.position, &tmp);
339
+
pts_relative_to_camera[i].x = tmp.x + move_amount.position.x;
340
+
pts_relative_to_camera[i].y = tmp.y + move_amount.position.y;
341
+
pts_relative_to_camera[i].z = tmp.z + move_amount.position.z;
342
+
343
+
pts_relative_to_camera[i].y *= -1;
344
+
pts_relative_to_camera[i].z *= -1;
345
+
346
+
if (pts_relative_to_camera[i].z < 0) {
347
+
any_joint_behind_camera = true;
348
+
}
349
+
}
350
+
351
+
352
+
std::vector<cv::Point2d> out(26);
353
+
//!@opencv_camera The OpenCV, it hurts
354
+
if (hgt->use_fisheye) {
355
+
cv::Affine3f aff = cv::Affine3f::Identity();
356
+
cv::fisheye::projectPoints(pts_relative_to_camera, out, aff, hgt->views[view_idx].cameraMatrix,
357
+
hgt->views[view_idx].distortion);
358
+
} else {
359
+
cv::Affine3d aff = cv::Affine3d::Identity();
360
+
// cv::Matx33d rvec = cv::Matx33d::
361
+
cv::Matx33d rotation = aff.rotation();
362
+
cv::projectPoints(pts_relative_to_camera, rotation, aff.translation(),
363
+
hgt->views[view_idx].cameraMatrix, hgt->views[view_idx].distortion, out);
364
+
}
365
+
xrt_vec2 keypoints_global[26];
366
+
bool outside_view[26] = {};
367
+
for (int i = 0; i < 26; i++) {
368
+
if (check_outside_view(hgt, hgt->image_boundary_info.views[view_idx], out[i]) ||
369
+
any_joint_behind_camera) {
370
+
outside_view[i] = true;
371
+
if (num_outside != NULL) {
372
+
num_outside[view_idx]++;
373
+
}
374
+
}
375
+
keypoints_global[i].x = out[i].x / hgt->multiply_px_coord_for_undistort;
376
+
keypoints_global[i].y = out[i].y / hgt->multiply_px_coord_for_undistort;
377
+
}
378
+
379
+
if (centers != NULL) {
380
+
centers[view_idx] = keypoints_global[XRT_HAND_JOINT_MIDDLE_PROXIMAL];
381
+
}
382
+
383
+
if (radii != NULL) {
384
+
for (int i = 0; i < 26; i++) {
385
+
radii[view_idx] =
386
+
std::max(radii[view_idx], m_vec2_len(centers[view_idx] - keypoints_global[i]));
387
+
}
388
+
radii[view_idx] *= hgt->tuneable_values.dyn_radii_fac.val;
389
+
}
390
+
391
+
392
+
393
+
cv::Mat debug = hgt->views[view_idx].debug_out_to_this;
394
+
if (also_debug_output) {
395
+
// for (int finger = 0; finger < 5; finger++) {
396
+
// cv::Point last = {(int)keypoints_global[0].x, (int)keypoints_global[0].y};
397
+
// for (int joint = 0; joint < 4; joint++) {
398
+
// cv::Point the_new = {(int)keypoints_global[1 + finger * 4 + joint].x,
399
+
// (int)keypoints_global[1 + finger * 4 + joint].y};
400
+
401
+
// cv::line(debug, last, the_new, colors[0]);
402
+
// last = the_new;
403
+
// }
404
+
// }
405
+
406
+
for (int i = 0; i < 26; i++) {
407
+
xrt_vec2 loc;
408
+
loc.x = keypoints_global[i].x;
409
+
loc.y = keypoints_global[i].y;
410
+
handDot(debug, loc, 2, outside_view[i] ? 0.0 : (float)(i) / 26.0, 1, 2);
411
+
}
412
+
}
413
+
} // for view_idx
414
+
}
415
+
416
+
261
417
static bool
262
418
handle_changed_image_size(HandTracking *hgt, xrt_size &new_one_view_size)
263
419
{
···
283
439
return true;
284
440
}
285
441
442
+
float
443
+
hand_confidence_value(float reprojection_error, one_frame_input &input)
444
+
{
445
+
float out_confidence = 0.0f;
446
+
for (int view_idx = 0; view_idx < 2; view_idx++) {
447
+
for (int i = 0; i < 21; i++) {
448
+
out_confidence += input.views[view_idx].confidences[i];
449
+
}
450
+
}
451
+
out_confidence /= 42.0f; // number of hand joints
452
+
float reproj_err_mul = 1.0f / (reprojection_error + 1.0f);
453
+
return out_confidence * reproj_err_mul;
454
+
}
455
+
456
+
457
+
xrt_vec3
458
+
correct_direction(xrt_vec2 in)
459
+
{
460
+
xrt_vec3 out = {in.x, -in.y, -1};
461
+
return m_vec3_normalize(out);
462
+
}
463
+
464
+
void
465
+
check_new_user_event(struct HandTracking *hgt)
466
+
{
467
+
if (hgt->tuneable_values.new_user_event) {
468
+
hgt->tuneable_values.new_user_event = false;
469
+
hgt->hand_seen_before[0] = false;
470
+
hgt->hand_seen_before[1] = false;
471
+
hgt->refinement.hand_size_refinement_schedule_x = 0;
472
+
}
473
+
}
474
+
475
+
bool
476
+
should_run_detection(struct HandTracking *hgt)
477
+
{
478
+
if (hgt->tuneable_values.always_run_detection_model) {
479
+
return true;
480
+
} else {
481
+
hgt->detection_counter++;
482
+
// Every 30 frames, but only if we aren't tracking both hands.
483
+
bool saw_both_hands_last_frame = hgt->last_frame_hand_detected[0] && hgt->last_frame_hand_detected[1];
484
+
return (hgt->detection_counter % 30 == 0) && !saw_both_hands_last_frame;
485
+
}
486
+
}
487
+
488
+
void
489
+
dispatch_and_process_hand_detections(struct HandTracking *hgt)
490
+
{
491
+
if (hgt->tuneable_values.always_run_detection_model) {
492
+
// Pretend like nothing was detected last frame.
493
+
for (int hand_idx = 0; hand_idx < 2; hand_idx++) {
494
+
// hgt->last_frame_hand_detected[hand_idx] = false;
495
+
hgt->this_frame_hand_detected[hand_idx] = false;
496
+
497
+
hgt->histories[hand_idx].hands.clear();
498
+
hgt->histories[hand_idx].timestamps.clear();
499
+
}
500
+
}
501
+
502
+
// view, hand
503
+
hand_bounding_box states[2][2] = {};
504
+
505
+
// paranoia
506
+
states[0]->found = false;
507
+
states[1]->found = false;
508
+
509
+
states[0]->confidence = 0;
510
+
states[1]->confidence = 0;
511
+
512
+
513
+
hand_detection_run_info infos[2] = {};
514
+
515
+
infos[0].view = &hgt->views[0];
516
+
infos[1].view = &hgt->views[1];
517
+
518
+
infos[0].outputs[0] = &states[0][0];
519
+
infos[0].outputs[1] = &states[0][1];
520
+
521
+
infos[1].outputs[0] = &states[1][0];
522
+
infos[1].outputs[1] = &states[1][1];
523
+
524
+
525
+
u_worker_group_push(hgt->group, run_hand_detection, &infos[0]);
526
+
u_worker_group_push(hgt->group, run_hand_detection, &infos[1]);
527
+
u_worker_group_wait_all(hgt->group);
528
+
for (int hand_idx = 0; hand_idx < 2; hand_idx++) {
529
+
if ((states[0][hand_idx].confidence + states[1][hand_idx].confidence) < 0.90) {
530
+
continue;
531
+
}
532
+
533
+
534
+
//!@todo Commented out the below code, which required all detections to be pointing at roughly the same
535
+
//! point in space.
536
+
// We should add this back, instead using lineline.cpp. But I gotta ship this, so we're just going to be
537
+
// less robust for now.
538
+
539
+
// xrt_vec2 in_left = raycoord(&hgt->views[0], states[0][hand_idx].center);
540
+
// xrt_vec2 in_right = raycoord(&hgt->views[1], states[1][hand_idx].center);
541
+
542
+
// xrt_vec2 dir_y_l = {in_left.y, -1.0f};
543
+
// xrt_vec2 dir_y_r = {in_right.y, -1.0f};
544
+
545
+
// m_vec2_normalize(&dir_y_l);
546
+
// m_vec2_normalize(&dir_y_r);
547
+
548
+
// float minimum = cosf(DEG_TO_RAD(10));
549
+
550
+
// float diff = m_vec2_dot(dir_y_l, dir_y_r);
551
+
552
+
// // U_LOG_E("diff %f", diff);
553
+
554
+
// if (diff < minimum) {
555
+
// HG_DEBUG(hgt,
556
+
// "Mismatch in detection models! Diff is %f, left Y axis is %f, right Y "
557
+
// "axis is %f",
558
+
// diff, in_left.y, in_right.y);
559
+
// continue;
560
+
// }
561
+
562
+
// If this hand was not detected last frame, we can add our prediction in.
563
+
// Or, if we're running the model every frame.
564
+
if (hgt->tuneable_values.always_run_detection_model || !hgt->last_frame_hand_detected[hand_idx]) {
565
+
hgt->views[0].bboxes_this_frame[hand_idx] = states[0][hand_idx];
566
+
hgt->views[1].bboxes_this_frame[hand_idx] = states[1][hand_idx];
567
+
}
568
+
569
+
570
+
hgt->this_frame_hand_detected[hand_idx] = true;
571
+
}
572
+
// Most of the time, this codepath runs - we predict where the hand should be based on the last
573
+
// two frames.
574
+
}
575
+
576
+
void
577
+
predict_new_regions_of_interest(struct HandTracking *hgt)
578
+
{
579
+
580
+
for (int hand_idx = 0; hand_idx < 2; hand_idx++) {
581
+
// If we don't have the past two frames, this code doesn't do what we want.
582
+
// If we only have *one* frame, we just reuse the same bounding box and hope the hand
583
+
// hasn't moved too much. @todo
584
+
585
+
if (hgt->histories[hand_idx].timestamps.size() < 2) {
586
+
HG_TRACE(hgt, "continuing, size is %zu", hgt->histories[hand_idx].timestamps.size());
587
+
continue;
588
+
}
589
+
590
+
// We can only do this *after* we know we're predicting - this would otherwise overwrite the detection
591
+
// model.
592
+
hgt->this_frame_hand_detected[hand_idx] = hgt->last_frame_hand_detected[hand_idx];
593
+
594
+
hand_history &history = hgt->histories[hand_idx];
595
+
uint64_t time_two_frames_ago = *history.timestamps.get_at_age(1);
596
+
uint64_t time_one_frame_ago = *history.timestamps.get_at_age(0);
597
+
uint64_t time_now = hgt->current_frame_timestamp;
598
+
599
+
xrt_hand_joint_set *set_two_frames_ago = history.hands.get_at_age(1);
600
+
xrt_hand_joint_set *set_one_frame_ago = history.hands.get_at_age(0);
601
+
602
+
// double dt_past = (double)() / (double)U_TIME_1S_IN_NS;
603
+
double dt_past = time_ns_to_s(time_one_frame_ago - time_two_frames_ago);
604
+
605
+
double dt_now = time_ns_to_s(time_now - time_one_frame_ago);
606
+
607
+
608
+
xrt_vec3 vels[26];
609
+
610
+
for (int i = 0; i < 26; i++) {
611
+
612
+
613
+
xrt_vec3 from_to = set_one_frame_ago->values.hand_joint_set_default[i].relation.pose.position -
614
+
set_two_frames_ago->values.hand_joint_set_default[i].relation.pose.position;
615
+
vels[i] = m_vec3_mul_scalar(from_to, 1.0 / dt_past);
616
+
617
+
// U_LOG_E("%f %f %f", vels[i].x, vels[i].y, vels[i].z);
618
+
}
619
+
xrt_vec3 positions_last_frame[26];
620
+
// xrt_vec3 predicted_positions_this_frame[26];
621
+
xrt_hand_joint_set predicted_positions_this_frame;
622
+
623
+
for (int i = 0; i < 26; i++) {
624
+
positions_last_frame[i] =
625
+
set_one_frame_ago->values.hand_joint_set_default[i].relation.pose.position;
626
+
627
+
//!@todo I dunno if this is right.
628
+
// Number of times this has been changed without rigorously testing: 1
629
+
float lerp_between_last_frame_and_predicted =
630
+
hgt->tuneable_values.amount_to_lerp_prediction.val;
631
+
predicted_positions_this_frame.values.hand_joint_set_default[i].relation.pose.position =
632
+
positions_last_frame[i] + (vels[i] * dt_now * lerp_between_last_frame_and_predicted);
633
+
}
634
+
xrt_vec2 centers[2] = {};
635
+
float radii[2] = {};
636
+
int num_outside[2] = {0, 0};
637
+
638
+
back_project( //
639
+
hgt, //
640
+
&predicted_positions_this_frame, //
641
+
hgt->tuneable_values.scribble_predictions_into_this_frame && hgt->debug_scribble, //
642
+
centers, //
643
+
radii, //
644
+
num_outside);
645
+
646
+
for (int view_idx = 0; view_idx < 2; view_idx++) {
647
+
if (num_outside[view_idx] < hgt->tuneable_values.max_num_outside_view) {
648
+
hgt->views[view_idx].bboxes_this_frame[hand_idx].center = centers[view_idx];
649
+
hgt->views[view_idx].bboxes_this_frame[hand_idx].size_px = radii[view_idx];
650
+
hgt->views[view_idx].bboxes_this_frame[hand_idx].found = true;
651
+
} else {
652
+
hgt->views[view_idx].bboxes_this_frame[hand_idx].found = false;
653
+
}
654
+
}
655
+
}
656
+
657
+
if (hgt->debug_scribble) {
658
+
659
+
for (int view_idx = 0; view_idx < 2; view_idx++) {
660
+
for (int hand_idx = 0; hand_idx < 2; hand_idx++) {
661
+
if (!hgt->views[view_idx].bboxes_this_frame[hand_idx].found) {
662
+
continue;
663
+
}
664
+
xrt_vec2 &_pt = hgt->views[view_idx].bboxes_this_frame[hand_idx].center;
665
+
float &size = hgt->views[view_idx].bboxes_this_frame[hand_idx].size_px;
666
+
cv::Point2i pt(_pt.x, _pt.y);
667
+
cv::rectangle(hgt->views[view_idx].debug_out_to_this,
668
+
cv::Rect(pt - cv::Point2i(size / 2, size / 2), cv::Size(size, size)),
669
+
colors[hand_idx], 1);
670
+
}
671
+
}
672
+
}
673
+
}
674
+
675
+
void
676
+
scribble_image_boundary(struct HandTracking *hgt)
677
+
{
678
+
for (int view_idx = 0; view_idx < 2; view_idx++) {
679
+
struct ht_view *view = &hgt->views[view_idx];
680
+
681
+
cv::Mat &debug_frame = view->debug_out_to_this;
682
+
hand_tracking_image_boundary_info_one_view &info = hgt->image_boundary_info.views[view_idx];
683
+
684
+
if (info.type == HT_IMAGE_BOUNDARY_CIRCLE) {
685
+
int center_x = hgt->last_frame_one_view_size_px.w * info.boundary.circle.normalized_center.x;
686
+
int center_y = hgt->last_frame_one_view_size_px.h * info.boundary.circle.normalized_center.y;
687
+
cv::circle(debug_frame, {center_x, center_y},
688
+
info.boundary.circle.normalized_radius * hgt->last_frame_one_view_size_px.w,
689
+
hsv2rgb(270.0, 0.5, 0.5), 2);
690
+
}
691
+
}
692
+
}
693
+
286
694
/*
287
695
*
288
696
* Member functions.
···
313
721
314
722
t_stereo_camera_calibration_reference(&this->calib, NULL);
315
723
316
-
free_kinematic_hand(&this->kinematic_hands[0]);
317
-
free_kinematic_hand(&this->kinematic_hands[1]);
724
+
lm::optimizer_destroy(&this->kinematic_hands[0]);
725
+
lm::optimizer_destroy(&this->kinematic_hands[1]);
726
+
727
+
ccdik::free_kinematic_hand(&this->kinematic_hands_ccdik[0]);
728
+
ccdik::free_kinematic_hand(&this->kinematic_hands_ccdik[1]);
318
729
319
730
u_var_remove_root((void *)&this->base);
320
731
u_frame_times_widget_teardown(&this->ft_widget);
···
352
763
xrt_size new_one_view_size;
353
764
new_one_view_size.h = left_frame->height;
354
765
new_one_view_size.w = left_frame->width;
355
-
// Could be an assert, should never happen.
766
+
// Could be an assert, should never happen after first frame.
356
767
if (!handle_changed_image_size(hgt, new_one_view_size)) {
357
768
return;
358
769
}
···
377
788
cv::Mat debug_output = {};
378
789
xrt_frame *debug_frame = nullptr;
379
790
380
-
791
+
// If we're outputting to a debug image, setup the image.
381
792
if (hgt->debug_scribble) {
382
793
u_frame_create_one_off(XRT_FORMAT_R8G8B8, full_width, full_height, &debug_frame);
383
794
debug_frame->timestamp = hgt->current_frame_timestamp;
···
391
802
392
803
hgt->views[0].debug_out_to_this = debug_output(cv::Rect(view_offsets[0], view_size));
393
804
hgt->views[1].debug_out_to_this = debug_output(cv::Rect(view_offsets[1], view_size));
805
+
scribble_image_boundary(hgt);
394
806
}
395
807
396
-
u_worker_group_push(hgt->group, run_hand_detection, &hgt->views[0]);
397
-
u_worker_group_push(hgt->group, run_hand_detection, &hgt->views[1]);
398
-
u_worker_group_wait_all(hgt->group);
399
-
808
+
check_new_user_event(hgt);
400
809
810
+
// Every now and then if we're not already tracking both hands, try to detect new hands.
811
+
if (should_run_detection(hgt)) {
812
+
dispatch_and_process_hand_detections(hgt);
813
+
}
814
+
// For already-tracked hands, predict where we think they should be in image space based on the past two
815
+
// frames. Note that this always happens We want to pose-predict already tracked hands but not mess with
816
+
// just-detected hands
817
+
if (!hgt->tuneable_values.always_run_detection_model) {
818
+
predict_new_regions_of_interest(hgt);
819
+
}
401
820
821
+
//!@todo does this go here?
822
+
// If no hand regions of interest were found anywhere, there's no hand - register that in the state tracker
402
823
for (int hand_idx = 0; hand_idx < 2; hand_idx++) {
403
-
404
-
if (!(hgt->views[0].det_outputs[hand_idx].found && hgt->views[1].det_outputs[hand_idx].found)) {
405
-
// If we don't find this hand in *both* views
406
-
out_xrt_hands[hand_idx]->is_active = false;
407
-
continue;
824
+
if (!(hgt->views[0].bboxes_this_frame[hand_idx].found ||
825
+
hgt->views[1].bboxes_this_frame[hand_idx].found)) {
826
+
hgt->this_frame_hand_detected[hand_idx] = false;
408
827
}
409
-
out_xrt_hands[hand_idx]->is_active = true;
828
+
}
410
829
411
830
831
+
// Dispatch keypoint estimator neural nets
832
+
for (int hand_idx = 0; hand_idx < 2; hand_idx++) {
412
833
for (int view_idx = 0; view_idx < 2; view_idx++) {
834
+
835
+
if (!hgt->views[view_idx].bboxes_this_frame[hand_idx].found) {
836
+
continue;
837
+
}
838
+
413
839
struct keypoint_estimation_run_info &inf = hgt->views[view_idx].run_info[hand_idx];
414
840
inf.view = &hgt->views[view_idx];
415
841
inf.hand_idx = hand_idx;
···
418
844
}
419
845
}
420
846
u_worker_group_wait_all(hgt->group);
847
+
848
+
849
+
// Spaghetti logic for optimizing hand size
850
+
bool any_hands_are_only_visible_in_one_view = false;
421
851
422
852
for (int hand_idx = 0; hand_idx < 2; hand_idx++) {
423
-
if (!out_xrt_hands[hand_idx]->is_active) {
424
-
hgt->last_frame_hand_detected[hand_idx] = false;
853
+
any_hands_are_only_visible_in_one_view = //
854
+
any_hands_are_only_visible_in_one_view || //
855
+
(hgt->views[0].bboxes_this_frame[hand_idx].found != //
856
+
hgt->views[1].bboxes_this_frame[hand_idx].found);
857
+
}
858
+
859
+
constexpr float mul_max = 1.0;
860
+
constexpr float frame_max = 100;
861
+
bool optimize_hand_size;
862
+
863
+
if ((hgt->refinement.hand_size_refinement_schedule_x > frame_max)) {
864
+
hgt->refinement.hand_size_refinement_schedule_y = mul_max;
865
+
optimize_hand_size = false;
866
+
} else {
867
+
hgt->refinement.hand_size_refinement_schedule_y =
868
+
powf((hgt->refinement.hand_size_refinement_schedule_x / frame_max), 2) * mul_max;
869
+
optimize_hand_size = true;
870
+
}
871
+
872
+
if (any_hands_are_only_visible_in_one_view) {
873
+
optimize_hand_size = false;
874
+
}
875
+
876
+
877
+
// if either hand was not visible before the last new-user event but is visible now, reset the schedule
878
+
// a bit.
879
+
if ((hgt->this_frame_hand_detected[0] && !hgt->hand_seen_before[0]) ||
880
+
(hgt->this_frame_hand_detected[1] && !hgt->hand_seen_before[1])) {
881
+
hgt->refinement.hand_size_refinement_schedule_x =
882
+
std::min(hgt->refinement.hand_size_refinement_schedule_x, frame_max / 2);
883
+
}
884
+
885
+
int num_hands = 0;
886
+
float avg_hand_size = 0;
887
+
888
+
// Dispatch the optimizers!
889
+
for (int hand_idx = 0; hand_idx < 2; hand_idx++) {
890
+
if (!hgt->this_frame_hand_detected[hand_idx]) {
425
891
continue;
426
892
}
427
-
kine::kinematic_hand_4f *hand = hgt->kinematic_hands[hand_idx];
428
-
if (!hgt->last_frame_hand_detected[hand_idx]) {
429
-
kine::init_hardcoded_statics(hand, hgt->hand_size / 100.0f);
893
+
894
+
one_frame_input input;
895
+
896
+
for (int view = 0; view < 2; view++) {
897
+
keypoint_output *from_model = &hgt->views[view].keypoint_outputs[hand_idx];
898
+
input.views[view].active = hgt->views[view].bboxes_this_frame[hand_idx].found;
899
+
if (!input.views[view].active) {
900
+
continue;
901
+
}
902
+
for (int i = 0; i < 21; i++) {
903
+
input.views[view].confidences[i] = from_model->hand_tan_space.confidences[i];
904
+
// std::cout << input.views[view].confidences[i] << std::endl;
905
+
input.views[view].rays[i] = correct_direction(from_model->hand_tan_space.kps[i]);
906
+
}
430
907
}
431
908
432
909
433
910
434
-
Hand2D *hand_in_left_view = &hgt->views[0].keypoint_outputs[hand_idx].hand_tan_space;
435
-
Hand2D *hand_in_right_view = &hgt->views[1].keypoint_outputs[hand_idx].hand_tan_space;
436
-
Hand3D hand_3d;
911
+
struct xrt_hand_joint_set *put_in_set = out_xrt_hands[hand_idx];
437
912
913
+
if (__builtin_expect(!hgt->tuneable_values.use_ccdik, true)) {
914
+
lm::KinematicHandLM *hand = hgt->kinematic_hands[hand_idx];
438
915
916
+
//!@todo
917
+
// ABOUT TWO MINUTES OF THOUGHT WERE PUT INTO THIS VALUE
918
+
float reprojection_error_threshold = 0.35f;
439
919
440
-
struct xrt_hand_joint_set *put_in_set = out_xrt_hands[hand_idx];
920
+
float out_hand_size;
441
921
442
-
applyThumbIndexDrag(&hand_3d);
922
+
//!@optimize We can have one of these on each thread
923
+
float reprojection_error;
924
+
lm::optimizer_run(hand, //
925
+
input, //
926
+
!hgt->last_frame_hand_detected[hand_idx], //
927
+
optimize_hand_size, //
928
+
hgt->target_hand_size, //
929
+
hgt->refinement.hand_size_refinement_schedule_y, //
930
+
*put_in_set, //
931
+
out_hand_size, //
932
+
reprojection_error);
443
933
444
-
applyJointWidths(hgt, put_in_set);
934
+
avg_hand_size += out_hand_size;
935
+
num_hands++;
936
+
937
+
if (reprojection_error > reprojection_error_threshold) {
938
+
HG_DEBUG(hgt, "Reprojection error above threshold!");
939
+
hgt->this_frame_hand_detected[hand_idx] = false;
940
+
941
+
continue;
942
+
}
943
+
if (!any_hands_are_only_visible_in_one_view) {
944
+
hgt->refinement.hand_size_refinement_schedule_x +=
945
+
hand_confidence_value(reprojection_error, input);
946
+
}
445
947
446
-
hgJointDisparityMath(hgt, hand_in_left_view, hand_in_right_view, &hand_3d);
948
+
} else {
949
+
ccdik::KinematicHandCCDIK *hand = hgt->kinematic_hands_ccdik[hand_idx];
950
+
if (!hgt->last_frame_hand_detected[hand_idx]) {
951
+
ccdik::init_hardcoded_statics(hand, hgt->target_hand_size);
952
+
}
953
+
ccdik::optimize_new_frame(hand, input, *put_in_set);
954
+
}
447
955
448
-
kine::optimize_new_frame(hand_3d.kps, hand, put_in_set, hand_idx);
449
956
450
957
451
-
math_pose_identity(&put_in_set->hand_pose.pose);
958
+
applyJointWidths(hgt, put_in_set);
452
959
453
-
switch (hgt->output_space) {
454
-
case HT_OUTPUT_SPACE_LEFT_CAMERA: {
455
-
put_in_set->hand_pose.pose.orientation = hgt->stereo_camera_to_left_camera;
456
-
} break;
457
-
case HT_OUTPUT_SPACE_CENTER_OF_STEREO_CAMERA: {
458
-
put_in_set->hand_pose.pose.orientation.w = 1.0;
459
-
put_in_set->hand_pose.pose.position.x = -hgt->baseline / 2;
460
-
} break;
960
+
// Just debug scribbling - remove this in hard production environment
961
+
if (hgt->tuneable_values.scribble_optimizer_outputs && hgt->debug_scribble) {
962
+
back_project(hgt, put_in_set, true, NULL, NULL, NULL);
461
963
}
462
964
965
+
put_in_set->hand_pose.pose = hgt->hand_pose_camera_offset;
463
966
put_in_set->hand_pose.relation_flags = valid_flags_ht;
967
+
968
+
969
+
hgt->histories[hand_idx].hands.push_back(*put_in_set);
970
+
hgt->histories[hand_idx].timestamps.push_back(hgt->current_frame_timestamp);
464
971
}
465
972
973
+
// More hand-size-optimization spaghetti
974
+
if (num_hands > 0) {
975
+
hgt->target_hand_size = (float)avg_hand_size / (float)num_hands;
976
+
}
466
977
978
+
// State tracker tweaks
979
+
for (int hand_idx = 0; hand_idx < 2; hand_idx++) {
980
+
out_xrt_hands[hand_idx]->is_active = hgt->this_frame_hand_detected[hand_idx];
981
+
hgt->last_frame_hand_detected[hand_idx] = hgt->this_frame_hand_detected[hand_idx];
467
982
983
+
hgt->hand_seen_before[hand_idx] =
984
+
hgt->hand_seen_before[hand_idx] || hgt->this_frame_hand_detected[hand_idx];
985
+
986
+
if (!hgt->last_frame_hand_detected[hand_idx]) {
987
+
hgt->views[0].bboxes_this_frame[hand_idx].found = false;
988
+
hgt->views[1].bboxes_this_frame[hand_idx].found = false;
989
+
hgt->histories[hand_idx].hands.clear();
990
+
hgt->histories[hand_idx].timestamps.clear();
991
+
}
992
+
}
993
+
994
+
// If the debug UI is active, push to the frame-timing widget
468
995
u_frame_times_widget_push_sample(&hgt->ft_widget, hgt->current_frame_timestamp);
469
996
997
+
// If the debug UI is active, push our debug frame
470
998
if (hgt->debug_scribble) {
471
999
u_sink_debug_push_frame(&hgt->debug_sink, debug_frame);
472
1000
xrt_frame_reference(&debug_frame, NULL);
473
1001
}
1002
+
1003
+
// done!
474
1004
}
475
1005
476
1006
void
···
483
1013
484
1014
} // namespace xrt::tracking::hand::mercury
485
1015
1016
+
1017
+
using namespace xrt::tracking::hand::mercury;
486
1018
487
1019
/*
488
1020
*
···
546
1078
hgt->views[0].hgt = hgt;
547
1079
hgt->views[1].hgt = hgt; // :)
548
1080
1081
+
hgt->image_boundary_info = boundary_info;
1082
+
549
1083
init_hand_detection(hgt, &hgt->views[0].detection);
550
1084
init_hand_detection(hgt, &hgt->views[1].detection);
551
1085
···
572
1106
hgt->pool = u_worker_thread_pool_create(num_threads - 1, num_threads);
573
1107
hgt->group = u_worker_group_create(hgt->pool);
574
1108
575
-
hgt->hand_size = 9.5864;
576
-
xrt::tracking::hand::mercury::kine::alloc_kinematic_hand(&hgt->kinematic_hands[0]);
577
-
xrt::tracking::hand::mercury::kine::alloc_kinematic_hand(&hgt->kinematic_hands[1]);
1109
+
lm::optimizer_create(hgt->left_in_right, false, hgt->log_level, &hgt->kinematic_hands[0]);
1110
+
lm::optimizer_create(hgt->left_in_right, true, hgt->log_level, &hgt->kinematic_hands[1]);
1111
+
1112
+
ccdik::alloc_kinematic_hand(hgt->left_in_right, false, &hgt->kinematic_hands_ccdik[0]);
1113
+
ccdik::alloc_kinematic_hand(hgt->left_in_right, true, &hgt->kinematic_hands_ccdik[1]);
578
1114
579
1115
u_frame_times_widget_init(&hgt->ft_widget, 10.0f, 10.0f);
580
1116
581
1117
u_var_add_root(hgt, "Camera-based Hand Tracker", true);
582
1118
583
1119
584
-
u_var_add_f32(hgt, &hgt->hand_size, "Hand size (Centimeters between wrist and middle-proximal joint)");
585
1120
u_var_add_ro_f32(hgt, &hgt->ft_widget.fps, "FPS!");
586
-
u_var_add_f32_timing(hgt, hgt->ft_widget.debug_var, "times!");
1121
+
u_var_add_f32_timing(hgt, hgt->ft_widget.debug_var, "Frame timing!");
1122
+
1123
+
u_var_add_ro_f32(hgt, &hgt->target_hand_size, "Hand size (Meters between wrist and middle-proximal joint)");
1124
+
u_var_add_ro_f32(hgt, &hgt->refinement.hand_size_refinement_schedule_x, "Schedule (X value)");
1125
+
u_var_add_ro_f32(hgt, &hgt->refinement.hand_size_refinement_schedule_y, "Schedule (Y value)");
1126
+
1127
+
1128
+
u_var_add_bool(hgt, &hgt->tuneable_values.new_user_event, "Trigger new-user event!");
1129
+
1130
+
hgt->tuneable_values.dyn_radii_fac.max = 4.0f;
1131
+
hgt->tuneable_values.dyn_radii_fac.min = 0.3f;
1132
+
hgt->tuneable_values.dyn_radii_fac.step = 0.02f;
1133
+
hgt->tuneable_values.dyn_radii_fac.val = 3.0f;
1134
+
1135
+
hgt->tuneable_values.dyn_joint_y_angle_error.max = 40.0f;
1136
+
hgt->tuneable_values.dyn_joint_y_angle_error.min = 0.0f;
1137
+
hgt->tuneable_values.dyn_joint_y_angle_error.step = 0.1f;
1138
+
hgt->tuneable_values.dyn_joint_y_angle_error.val = 10.0f;
1139
+
1140
+
// Number of times this has been changed without rigorously testing: 1
1141
+
hgt->tuneable_values.amount_to_lerp_prediction.max = 1.5f;
1142
+
hgt->tuneable_values.amount_to_lerp_prediction.min = -1.5f;
1143
+
hgt->tuneable_values.amount_to_lerp_prediction.step = 0.01f;
1144
+
hgt->tuneable_values.amount_to_lerp_prediction.val = 0.4f;
1145
+
1146
+
1147
+
u_var_add_draggable_f32(hgt, &hgt->tuneable_values.dyn_radii_fac, "radius factor (predict)");
1148
+
u_var_add_draggable_f32(hgt, &hgt->tuneable_values.dyn_joint_y_angle_error, "max error hand joint");
1149
+
u_var_add_draggable_f32(hgt, &hgt->tuneable_values.amount_to_lerp_prediction, "Amount to lerp pose-prediction");
1150
+
1151
+
u_var_add_bool(hgt, &hgt->tuneable_values.scribble_predictions_into_this_frame, "Scribble pose-predictions");
1152
+
u_var_add_bool(hgt, &hgt->tuneable_values.scribble_keypoint_model_outputs, "Scribble keypoint model output");
1153
+
u_var_add_bool(hgt, &hgt->tuneable_values.scribble_optimizer_outputs, "Scribble kinematic optimizer output");
1154
+
u_var_add_bool(hgt, &hgt->tuneable_values.always_run_detection_model, "Always run detection model");
1155
+
u_var_add_bool(hgt, &hgt->tuneable_values.use_ccdik,
1156
+
"Use IK optimizer (may put tracking in unexpected state, use with care)");
1157
+
587
1158
588
1159
u_var_add_sink_debug(hgt, &hgt->debug_sink, "i");
589
1160
+110
-39
src/xrt/tracking/hand/mercury/hg_sync.hpp
+110
-39
src/xrt/tracking/hand/mercury/hg_sync.hpp
···
14
14
15
15
#include "tracking/t_calibration_opencv.hpp"
16
16
17
+
#include "tracking/t_hand_tracking.h"
17
18
#include "xrt/xrt_defines.h"
18
19
#include "xrt/xrt_frame.h"
19
20
···
32
33
#include "util/u_frame.h"
33
34
#include "util/u_var.h"
34
35
36
+
#include "util/u_template_historybuf.hpp"
37
+
35
38
#include <assert.h>
36
39
#include <stdio.h>
37
40
#include <stdlib.h>
···
41
44
#include <opencv2/opencv.hpp>
42
45
#include <onnxruntime_c_api.h>
43
46
44
-
#include "kine/kinematic_interface.hpp"
47
+
#include "kine_common.hpp"
48
+
#include "kine_lm/lm_interface.hpp"
49
+
#include "kine_ccdik/ccdik_interface.hpp"
50
+
45
51
46
52
47
53
namespace xrt::tracking::hand::mercury {
48
54
49
55
using namespace xrt::auxiliary::util;
50
56
51
-
DEBUG_GET_ONCE_LOG_OPTION(mercury_log, "MERCURY_LOG", U_LOGGING_WARN)
52
-
DEBUG_GET_ONCE_BOOL_OPTION(mercury_use_simdr_keypoint, "MERCURY_USE_SIMDR_KEYPOINT", false)
53
-
54
57
#define HG_TRACE(hgt, ...) U_LOG_IFL_T(hgt->log_level, __VA_ARGS__)
55
58
#define HG_DEBUG(hgt, ...) U_LOG_IFL_D(hgt->log_level, __VA_ARGS__)
56
59
#define HG_INFO(hgt, ...) U_LOG_IFL_I(hgt->log_level, __VA_ARGS__)
57
60
#define HG_WARN(hgt, ...) U_LOG_IFL_W(hgt->log_level, __VA_ARGS__)
58
61
#define HG_ERROR(hgt, ...) U_LOG_IFL_E(hgt->log_level, __VA_ARGS__)
59
62
63
+
64
+
static const cv::Scalar RED(255, 30, 30);
65
+
static const cv::Scalar YELLOW(255, 255, 0);
66
+
static const cv::Scalar PINK(255, 0, 255);
67
+
68
+
static const cv::Scalar colors[2] = {YELLOW, RED};
69
+
60
70
#undef USE_NCNN
61
71
62
72
// Forward declaration for ht_view
63
73
struct HandTracking;
64
74
struct ht_view;
65
75
66
-
enum Joint21
67
-
{
68
-
WRIST = 0,
69
-
70
-
THMB_MCP = 1,
71
-
THMB_PXM = 2,
72
-
THMB_DST = 3,
73
-
THMB_TIP = 4,
74
-
75
-
INDX_PXM = 5,
76
-
INDX_INT = 6,
77
-
INDX_DST = 7,
78
-
INDX_TIP = 8,
79
-
80
-
MIDL_PXM = 9,
81
-
MIDL_INT = 10,
82
-
MIDL_DST = 11,
83
-
MIDL_TIP = 12,
84
-
85
-
RING_PXM = 13,
86
-
RING_INT = 14,
87
-
RING_DST = 15,
88
-
RING_TIP = 16,
89
-
90
-
LITL_PXM = 17,
91
-
LITL_INT = 18,
92
-
LITL_DST = 19,
93
-
LITL_TIP = 20
94
-
};
95
-
96
76
// Using the compiler to stop me from getting 2D space mixed up with 3D space.
97
77
struct Hand2D
98
78
{
99
79
struct xrt_vec2 kps[21];
80
+
float confidences[21];
100
81
};
101
82
102
83
struct Hand3D
···
117
98
const char *input_name;
118
99
};
119
100
120
-
struct det_output
101
+
struct hand_bounding_box
121
102
{
122
103
xrt_vec2 center;
123
104
float size_px;
124
105
bool found;
106
+
bool confidence;
125
107
};
126
108
109
+
struct hand_detection_run_info
110
+
{
111
+
ht_view *view;
112
+
hand_bounding_box *outputs[2];
113
+
};
114
+
115
+
127
116
struct keypoint_output
128
117
{
129
118
Hand2D hand_px_coord;
130
119
Hand2D hand_tan_space;
120
+
float confidences[21];
131
121
};
132
122
133
123
struct keypoint_estimation_run_info
···
150
140
cv::Mat run_model_on_this;
151
141
cv::Mat debug_out_to_this;
152
142
153
-
struct det_output det_outputs[2]; // left, right
143
+
struct hand_bounding_box bboxes_this_frame[2]; // left, right
144
+
154
145
struct keypoint_estimation_run_info run_info[2];
155
146
156
147
struct keypoint_output keypoint_outputs[2];
157
148
};
158
149
150
+
struct hand_history
151
+
{
152
+
HistoryBuffer<xrt_hand_joint_set, 5> hands;
153
+
HistoryBuffer<uint64_t, 5> timestamps;
154
+
};
155
+
156
+
struct output_struct_one_frame
157
+
{
158
+
xrt_vec2 left[21];
159
+
float confidences_left[21];
160
+
xrt_vec2 right[21];
161
+
float confidences_right[21];
162
+
};
163
+
164
+
struct hand_size_refinement
165
+
{
166
+
int num_hands;
167
+
float out_hand_size;
168
+
float out_hand_confidence;
169
+
float hand_size_refinement_schedule_x = 0;
170
+
float hand_size_refinement_schedule_y = 0;
171
+
};
172
+
159
173
/*!
160
174
* Main class of Mercury hand tracking.
161
175
*
···
193
207
194
208
u_worker_group *group;
195
209
196
-
float hand_size;
197
210
198
211
float baseline = {};
199
-
struct xrt_quat stereo_camera_to_left_camera = {};
212
+
xrt_pose hand_pose_camera_offset = {};
200
213
201
214
uint64_t current_frame_timestamp = {};
202
215
···
207
220
208
221
enum u_logging_level log_level = U_LOGGING_INFO;
209
222
210
-
kine::kinematic_hand_4f *kinematic_hands[2];
223
+
lm::KinematicHandLM *kinematic_hands[2];
224
+
ccdik::KinematicHandCCDIK *kinematic_hands_ccdik[2];
225
+
226
+
// struct hand_detection_state_tracker st_det[2] = {};
227
+
bool hand_seen_before[2] = {false, false};
211
228
bool last_frame_hand_detected[2] = {false, false};
229
+
bool this_frame_hand_detected[2] = {false, false};
230
+
231
+
struct hand_history histories[2];
232
+
233
+
int detection_counter = 0;
234
+
235
+
struct hand_size_refinement refinement = {};
236
+
// Moses hand size is ~0.095; they has big-ish hands so let's do 0.09
237
+
float target_hand_size = 0.09;
238
+
212
239
213
240
xrt_frame *debug_frame;
214
241
215
242
void (*keypoint_estimation_run_func)(void *);
216
243
244
+
245
+
246
+
struct xrt_pose left_in_right = {};
247
+
struct hand_tracking_image_boundary_info image_boundary_info;
248
+
217
249
u_frame_times_widget ft_widget = {};
218
250
251
+
struct
252
+
{
253
+
bool new_user_event = false;
254
+
struct u_var_draggable_f32 dyn_radii_fac;
255
+
struct u_var_draggable_f32 dyn_joint_y_angle_error;
256
+
struct u_var_draggable_f32 amount_to_lerp_prediction;
257
+
bool scribble_predictions_into_this_frame = false;
258
+
bool scribble_keypoint_model_outputs = false;
259
+
bool scribble_optimizer_outputs = true;
260
+
bool always_run_detection_model = false;
261
+
bool use_ccdik = false;
262
+
int max_num_outside_view = 3;
263
+
} tuneable_values;
264
+
265
+
219
266
public:
220
267
explicit HandTracking();
221
268
~HandTracking();
···
237
284
static void
238
285
cCallbackDestroy(t_hand_tracking_sync *ht_sync);
239
286
};
287
+
288
+
289
+
void
290
+
init_hand_detection(HandTracking *hgt, onnx_wrap *wrap);
291
+
292
+
void
293
+
run_hand_detection(void *ptr);
294
+
295
+
void
296
+
init_keypoint_estimation(HandTracking *hgt, onnx_wrap *wrap);
297
+
298
+
299
+
void
300
+
run_keypoint_estimation(void *ptr);
301
+
302
+
303
+
void
304
+
init_keypoint_estimation_new(HandTracking *hgt, onnx_wrap *wrap);
305
+
306
+
void
307
+
run_keypoint_estimation_new(void *ptr);
308
+
309
+
void
310
+
release_onnx_wrap(onnx_wrap *wrap);
240
311
241
312
} // namespace xrt::tracking::hand::mercury
+28
-61
src/xrt/tracking/hand/mercury/kine/kinematic_defines.hpp
src/xrt/tracking/hand/mercury/kine_ccdik/ccdik_defines.hpp
+28
-61
src/xrt/tracking/hand/mercury/kine/kinematic_defines.hpp
src/xrt/tracking/hand/mercury/kine_ccdik/ccdik_defines.hpp
···
35
35
36
36
#include <stddef.h>
37
37
#include <unistd.h>
38
+
#include "../kine_common.hpp"
38
39
39
40
40
41
using namespace xrt::auxiliary::math;
41
42
42
-
namespace xrt::tracking::hand::mercury::kine {
43
-
44
-
// Not doing enum class, I *want* to allow implicit conversions.
45
-
namespace Joint21 {
46
-
enum Joint21
47
-
{
48
-
WRIST = 0,
49
-
50
-
THMB_MCP = 1,
51
-
THMB_PXM = 2,
52
-
THMB_DST = 3,
53
-
THMB_TIP = 4,
54
-
55
-
INDX_PXM = 5,
56
-
INDX_INT = 6,
57
-
INDX_DST = 7,
58
-
INDX_TIP = 8,
59
-
60
-
MIDL_PXM = 9,
61
-
MIDL_INT = 10,
62
-
MIDL_DST = 11,
63
-
MIDL_TIP = 12,
64
-
65
-
RING_PXM = 13,
66
-
RING_INT = 14,
67
-
RING_DST = 15,
68
-
RING_TIP = 16,
69
-
70
-
LITL_PXM = 17,
71
-
LITL_INT = 18,
72
-
LITL_DST = 19,
73
-
LITL_TIP = 20
74
-
};
75
-
}
43
+
namespace xrt::tracking::hand::mercury::ccdik {
76
44
77
45
enum class HandJoint26KP : int
78
46
{
···
135
103
TB_DISTAL
136
104
};
137
105
106
+
const size_t kNumNNJoints = 21;
107
+
138
108
struct wct_t
139
109
{
140
-
float waggle;
141
-
float curl;
142
-
float twist;
110
+
float waggle = 0.0f;
111
+
float curl = 0.0f;
112
+
float twist = 0.0f;
143
113
};
144
114
145
115
// IGNORE THE FIRST BONE in the wrist.
146
116
struct bone_t
147
117
{
118
+
// EIGEN_OVERRIDE_OPERATOR_NEW
119
+
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
148
120
// will always be 0, 0, -(some amount) for mcp, pxm, int, dst
149
121
// will be random amounts for carpal bones
150
-
Eigen::Vector3f trans_from_last_joint;
151
-
wct_t rot_to_next_joint_wct;
152
-
Eigen::Quaternionf rot_to_next_joint_quat;
122
+
Eigen::Vector3f trans_from_last_joint = Eigen::Vector3f::Zero();
123
+
wct_t rot_to_next_joint_wct = {};
124
+
Eigen::Quaternionf rot_to_next_joint_quat = {};
153
125
// Translation from last joint to this joint, rotation that takes last joint's -z and points it at next joint
154
-
Eigen::Affine3f bone_relation;
126
+
Eigen::Affine3f bone_relation = {};
155
127
156
128
// Imagine it like transforming an object at the origin to this bone's position/orientation.
157
-
Eigen::Affine3f world_pose;
129
+
Eigen::Affine3f world_pose = {};
158
130
159
131
struct
160
132
{
···
163
135
} parent;
164
136
165
137
166
-
wct_t joint_limit_min;
167
-
wct_t joint_limit_max;
138
+
wct_t joint_limit_min = {};
139
+
wct_t joint_limit_max = {};
168
140
169
141
170
142
// What keypoint out of the ML model does this correspond to?
171
-
Joint21::Joint21 keypoint_idx_21;
172
-
173
-
// float static_weight
174
-
// float model_confidence_weight ?
143
+
Joint21::Joint21 keypoint_idx_21 = {};
175
144
};
176
145
177
146
// translation: wrist to mcp (-z and x). rotation: from wrist space to metacarpal space
···
179
148
180
149
struct finger_t
181
150
{
182
-
bone_t bones[5];
151
+
bone_t bones[5] = {};
183
152
};
184
153
185
154
186
-
typedef struct kinematic_hand_4f
155
+
struct KinematicHandCCDIK
187
156
{
188
157
// The distance from the wrist to the middle-proximal joint - this sets the overall hand size.
189
158
float size;
159
+
bool is_right;
160
+
xrt_pose right_in_left;
190
161
191
162
// Wrist pose, scaled by size.
192
-
Eigen::Affine3f wrist_relation;
193
-
194
-
finger_t fingers[5];
195
-
196
-
xrt_vec3 t_jts[21];
197
-
Eigen::Matrix<float, 3, 21> t_jts_as_mat;
198
-
Eigen::Matrix<float, 3, 21> kinematic;
163
+
Eigen::Affine3f wrist_relation = {};
199
164
200
-
// // Pointers to the locations of
201
-
// struct xrt_vec3 *loc_ptrs[21];
165
+
finger_t fingers[5] = {};
202
166
203
-
} kinematic_hand_4f;
167
+
xrt_vec3 t_jts[kNumNNJoints] = {};
168
+
Eigen::Matrix<float, 3, kNumNNJoints> t_jts_as_mat = {};
169
+
Eigen::Matrix<float, 3, kNumNNJoints> kinematic = {};
170
+
};
204
171
205
172
206
173
#define CONTINUE_IF_HIDDEN_THUMB \
···
208
175
continue; \
209
176
}
210
177
211
-
} // namespace xrt::tracking::hand::mercury::kine
178
+
} // namespace xrt::tracking::hand::mercury::ccdik
+12
-10
src/xrt/tracking/hand/mercury/kine/kinematic_hand_init.hpp
src/xrt/tracking/hand/mercury/kine_ccdik/ccdik_hand_init.hpp
+12
-10
src/xrt/tracking/hand/mercury/kine/kinematic_hand_init.hpp
src/xrt/tracking/hand/mercury/kine_ccdik/ccdik_hand_init.hpp
···
10
10
11
11
#pragma once
12
12
13
-
#include "kinematic_defines.hpp"
14
-
#include "kinematic_tiny_math.hpp"
13
+
#include "ccdik_defines.hpp"
14
+
#include "ccdik_tiny_math.hpp"
15
15
16
16
17
-
namespace xrt::tracking::hand::mercury::kine {
17
+
namespace xrt::tracking::hand::mercury::ccdik {
18
18
19
19
void
20
20
bone_update_quat_and_matrix(struct bone_t *bone)
···
39
39
}
40
40
41
41
void
42
-
_statics_init_world_parents(kinematic_hand_4f *hand)
42
+
_statics_init_world_parents(KinematicHandCCDIK *hand)
43
43
{
44
44
for (int finger = 0; finger < 5; finger++) {
45
45
finger_t *of = &hand->fingers[finger];
···
56
56
}
57
57
58
58
void
59
-
_statics_init_world_poses(kinematic_hand_4f *hand)
59
+
_statics_init_world_poses(KinematicHandCCDIK *hand)
60
60
{
61
61
XRT_TRACE_MARKER();
62
62
for (int finger = 0; finger < 5; finger++) {
···
70
70
}
71
71
72
72
void
73
-
_statics_init_loc_ptrs(kinematic_hand_4f *hand)
73
+
_statics_init_loc_ptrs(KinematicHandCCDIK *hand)
74
74
{
75
75
hand->fingers[0].bones[1].keypoint_idx_21 = Joint21::THMB_MCP;
76
76
hand->fingers[0].bones[2].keypoint_idx_21 = Joint21::THMB_PXM;
···
99
99
}
100
100
101
101
void
102
-
_statics_joint_limits(kinematic_hand_4f *hand)
102
+
_statics_joint_limits(KinematicHandCCDIK *hand)
103
103
{
104
104
{
105
105
finger_t *t = &hand->fingers[0];
···
138
138
139
139
// Exported:
140
140
void
141
-
init_hardcoded_statics(kinematic_hand_4f *hand, float size)
141
+
init_hardcoded_statics(KinematicHandCCDIK *hand, float size)
142
142
{
143
-
memset(hand, 0, sizeof(kinematic_hand_4f));
144
143
hand->size = size;
145
144
hand->wrist_relation.setIdentity();
146
145
hand->wrist_relation.linear() *= hand->size;
···
202
201
203
202
for (int i = 0; i < 3; i++) {
204
203
int bone = i + 2;
204
+
of->bones[bone].trans_from_last_joint.x() = 0;
205
+
of->bones[bone].trans_from_last_joint.y() = 0;
205
206
of->bones[bone].trans_from_last_joint.z() = finger_joints[finger - 1][i];
206
207
}
207
208
}
209
+
208
210
209
211
hand->fingers[1].bones[1].trans_from_last_joint.z() = -0.66;
210
212
hand->fingers[2].bones[1].trans_from_last_joint.z() = -0.645;
···
227
229
_statics_init_loc_ptrs(hand);
228
230
_statics_joint_limits(hand);
229
231
}
230
-
} // namespace xrt::tracking::hand::mercury::kine
232
+
} // namespace xrt::tracking::hand::mercury::ccdik
-34
src/xrt/tracking/hand/mercury/kine/kinematic_interface.hpp
-34
src/xrt/tracking/hand/mercury/kine/kinematic_interface.hpp
···
1
-
// Copyright 2022, Collabora, Ltd.
2
-
// SPDX-License-Identifier: BSL-1.0
3
-
/*!
4
-
* @file
5
-
* @brief Interface for kinematic model
6
-
* @author Moses Turner <moses@collabora.com>
7
-
* @ingroup tracking
8
-
*/
9
-
#pragma once
10
-
#include "math/m_api.h"
11
-
12
-
#include "xrt/xrt_defines.h"
13
-
14
-
15
-
namespace xrt::tracking::hand::mercury::kine {
16
-
17
-
struct kinematic_hand_4f;
18
-
19
-
void
20
-
alloc_kinematic_hand(kinematic_hand_4f **out_kinematic_hand);
21
-
22
-
void
23
-
optimize_new_frame(xrt_vec3 model_joints_3d[21],
24
-
kinematic_hand_4f *hand,
25
-
struct xrt_hand_joint_set *out_set,
26
-
int hand_idx);
27
-
28
-
void
29
-
init_hardcoded_statics(kinematic_hand_4f *hand, float size = 0.095864);
30
-
31
-
void
32
-
free_kinematic_hand(kinematic_hand_4f **kinematic_hand);
33
-
34
-
} // namespace xrt::tracking::hand::mercury::kine
+85
-119
src/xrt/tracking/hand/mercury/kine/kinematic_main.cpp
src/xrt/tracking/hand/mercury/kine_ccdik/ccdik_main.cpp
+85
-119
src/xrt/tracking/hand/mercury/kine/kinematic_main.cpp
src/xrt/tracking/hand/mercury/kine_ccdik/ccdik_main.cpp
···
7
7
* @ingroup tracking
8
8
*/
9
9
10
-
#include "kinematic_defines.hpp"
11
-
#include "kinematic_tiny_math.hpp"
12
-
#include "kinematic_hand_init.hpp"
10
+
#include "ccdik_defines.hpp"
11
+
#include "ccdik_tiny_math.hpp"
12
+
#include "ccdik_hand_init.hpp"
13
+
#include "lineline.hpp"
14
+
#include "math/m_api.h"
15
+
#include "util/u_logging.h"
13
16
14
17
#include <Eigen/Core>
15
18
#include <Eigen/LU>
···
18
21
19
22
20
23
21
-
namespace xrt::tracking::hand::mercury::kine {
24
+
namespace xrt::tracking::hand::mercury::ccdik {
22
25
23
26
static void
24
-
_two_set_ele(Eigen::Matrix<float, 3, 21> &thing, Eigen::Affine3f jt, int idx)
27
+
_two_set_ele(Eigen::Matrix<float, 3, kNumNNJoints> &thing, Eigen::Affine3f jt, int idx)
25
28
{
26
-
// slow
29
+
//! @optimize
27
30
thing.col(idx) = jt.translation();
28
31
}
29
32
30
33
static void
31
-
two(struct kinematic_hand_4f *hand)
34
+
two(struct KinematicHandCCDIK *hand)
32
35
{
33
36
XRT_TRACE_MARKER();
34
37
···
105
108
106
109
107
110
static void
108
-
do_it_for_bone(struct kinematic_hand_4f *hand, int finger_idx, int bone_idx, bool clamp_to_x_axis_rotation)
111
+
do_it_for_bone(struct KinematicHandCCDIK *hand, int finger_idx, int bone_idx, bool clamp_to_x_axis_rotation)
109
112
{
110
113
finger_t *of = &hand->fingers[finger_idx];
111
114
bone_t *bone = &hand->fingers[finger_idx].bones[bone_idx];
···
135
138
bone->bone_relation.linear() = bone->bone_relation.linear() * rot;
136
139
}
137
140
138
-
#if 1
139
141
static void
140
-
clamp_to_x_axis(struct kinematic_hand_4f *hand,
141
-
int finger_idx,
142
-
int bone_idx,
143
-
bool clamp_angle = false,
144
-
float min_angle = 0,
145
-
float max_angle = 0)
146
-
{
147
-
bone_t &bone = hand->fingers[finger_idx].bones[bone_idx];
148
-
// U_LOG_E("before_anything");
149
-
150
-
// std::cout << bone->bone_relation.linear().matrix() << std::endl;
151
-
int axis = 0;
152
-
Eigen::Vector3f axes[3] = {Eigen::Vector3f::UnitX(), Eigen::Vector3f::UnitY(), Eigen::Vector3f::UnitZ()};
153
-
154
-
155
-
// The input rotation will very likely rotate a vector pointed in the direction of axis we want to lock to...
156
-
// somewhere else. So, we find the new direction
157
-
Eigen::Vector3f axis_rotated_by_input = bone.bone_relation.linear() * axes[axis];
158
-
159
-
// And find a correction rotation that rotates our input rotation such that it doesn't affect vectors pointing
160
-
// in the direction of our axis anymore.
161
-
Eigen::Matrix3f correction_rot =
162
-
Eigen::Quaternionf().setFromTwoVectors(axis_rotated_by_input.normalized(), axes[axis]).matrix();
163
-
bone.bone_relation.linear() = correction_rot * bone.bone_relation.linear();
164
-
165
-
166
-
if (!clamp_angle) {
167
-
return;
168
-
}
169
-
170
-
Eigen::Vector3f axis_to_clamp_rotation = axes[(axis + 1) % 3];
171
-
172
-
Eigen::Vector3f new_ortho_direction = bone.bone_relation.linear() * axis_to_clamp_rotation;
173
-
float rotation_value = atan2(new_ortho_direction((axis + 2) % 3), new_ortho_direction((axis + 1) % 3));
174
-
175
-
176
-
177
-
if (rotation_value < max_angle && rotation_value > min_angle) {
178
-
return;
179
-
}
180
-
181
-
float positive_rotation_value, negative_rotation_value;
182
-
183
-
184
-
if (rotation_value < 0) {
185
-
positive_rotation_value = (M_PI * 2) - rotation_value;
186
-
negative_rotation_value = rotation_value;
187
-
} else {
188
-
negative_rotation_value = (-M_PI * 2) + rotation_value;
189
-
positive_rotation_value = rotation_value;
190
-
}
191
-
192
-
if ((positive_rotation_value - max_angle) > (min_angle - negative_rotation_value)) {
193
-
// Difference between max angle and positive value is higher, so we're closer to the minimum bound.
194
-
rotation_value = min_angle;
195
-
} else {
196
-
rotation_value = max_angle;
197
-
}
198
-
199
-
bone.bone_relation.linear() = Eigen::AngleAxisf(rotation_value, axes[axis]).toRotationMatrix();
200
-
}
201
-
202
-
#else
203
-
static void
204
-
clamp_to_x_axis(struct kinematic_hand_4f *hand,
142
+
clamp_to_x_axis(struct KinematicHandCCDIK *hand,
205
143
int finger_idx,
206
144
int bone_idx,
207
145
bool clamp_angle = false,
···
242
180
// std::cout << bone->bone_relation.linear() * Eigen::Vector3f::UnitX() << "\n";
243
181
244
182
if (clamp_angle) {
245
-
//! @todo optimize: get rid of 1 and 2, we only need 0.
183
+
//! @optimize: get rid of 1 and 2, we only need 0.
246
184
247
185
// signed angle: asin(Cross product of -z and rot*-z X axis.
248
186
// U_LOG_E("before X clamp");
···
262
200
263
201
264
202
265
-
//! @todo optimize: Move the asin into constexpr land
203
+
//! @optimize: Move the asin into constexpr land
266
204
// No, the sine of the joint limit
267
205
float rotation_value = asin(cross(0));
268
206
···
282
220
// std::cout << n << "\n";
283
221
}
284
222
}
285
-
#endif
286
-
287
223
288
224
// Is this not just swing-twist about the Z axis? Dunnoooo... Find out later.
289
225
static void
290
-
clamp_proximals(struct kinematic_hand_4f *hand,
226
+
clamp_proximals(struct KinematicHandCCDIK *hand,
291
227
int finger_idx,
292
228
int bone_idx,
293
229
float max_swing_angle = 0,
···
343
279
344
280
345
281
if (our_z.z() > 0) {
346
-
//!@bug We need smarter joint limiting, limiting using tanangles is not acceptable as joints can rotate
282
+
//!@bug We need smarter joint limiting, limiting via tanangles is not acceptable as joints can rotate
347
283
//! outside of the 180 degree hemisphere.
348
284
our_z.z() = -0.000001f;
349
285
}
···
361
297
362
298
363
299
static void
364
-
do_it_for_finger(struct kinematic_hand_4f *hand, int finger_idx)
300
+
do_it_for_finger(struct KinematicHandCCDIK *hand, int finger_idx)
365
301
{
366
302
do_it_for_bone(hand, finger_idx, 0, false);
367
303
clamp_proximals(hand, finger_idx, 0, rad(4.0), tan(rad(-30)), tan(rad(30)), tan(rad(-10)), tan(rad(10)));
···
382
318
}
383
319
384
320
static void
385
-
optimize(kinematic_hand_4f *hand)
321
+
optimize(KinematicHandCCDIK *hand)
386
322
{
387
323
for (int i = 0; i < 15; i++) {
388
324
two(hand);
···
410
346
411
347
412
348
static void
413
-
make_joint_at_matrix_left_hand(int idx, Eigen::Affine3f &pose, struct xrt_hand_joint_set *hand)
349
+
make_joint_at_matrix_left_hand(int idx, Eigen::Affine3f &pose, struct xrt_hand_joint_set &hand)
414
350
{
415
-
hand->values.hand_joint_set_default[idx].relation.relation_flags = (enum xrt_space_relation_flags)(
351
+
hand.values.hand_joint_set_default[idx].relation.relation_flags = (enum xrt_space_relation_flags)(
416
352
XRT_SPACE_RELATION_ORIENTATION_VALID_BIT | XRT_SPACE_RELATION_ORIENTATION_TRACKED_BIT |
417
353
XRT_SPACE_RELATION_POSITION_VALID_BIT | XRT_SPACE_RELATION_POSITION_TRACKED_BIT);
418
354
Eigen::Vector3f v = pose.translation();
419
-
hand->values.hand_joint_set_default[idx].relation.pose.position.x = v.x();
420
-
hand->values.hand_joint_set_default[idx].relation.pose.position.y = v.y();
421
-
hand->values.hand_joint_set_default[idx].relation.pose.position.z = v.z();
355
+
hand.values.hand_joint_set_default[idx].relation.pose.position.x = v.x();
356
+
hand.values.hand_joint_set_default[idx].relation.pose.position.y = v.y();
357
+
hand.values.hand_joint_set_default[idx].relation.pose.position.z = v.z();
422
358
423
359
Eigen::Quaternionf q;
424
360
q = pose.rotation();
425
361
426
-
hand->values.hand_joint_set_default[idx].relation.pose.orientation.x = q.x();
427
-
hand->values.hand_joint_set_default[idx].relation.pose.orientation.y = q.y();
428
-
hand->values.hand_joint_set_default[idx].relation.pose.orientation.z = q.z();
429
-
hand->values.hand_joint_set_default[idx].relation.pose.orientation.w = q.w();
362
+
hand.values.hand_joint_set_default[idx].relation.pose.orientation.x = q.x();
363
+
hand.values.hand_joint_set_default[idx].relation.pose.orientation.y = q.y();
364
+
hand.values.hand_joint_set_default[idx].relation.pose.orientation.z = q.z();
365
+
hand.values.hand_joint_set_default[idx].relation.pose.orientation.w = q.w();
430
366
}
431
367
432
368
static void
433
-
make_joint_at_matrix_right_hand(int idx, Eigen::Affine3f &pose, struct xrt_hand_joint_set *hand)
369
+
make_joint_at_matrix_right_hand(int idx, Eigen::Affine3f &pose, struct xrt_hand_joint_set &hand)
434
370
{
435
-
hand->values.hand_joint_set_default[idx].relation.relation_flags = (enum xrt_space_relation_flags)(
371
+
hand.values.hand_joint_set_default[idx].relation.relation_flags = (enum xrt_space_relation_flags)(
436
372
XRT_SPACE_RELATION_ORIENTATION_VALID_BIT | XRT_SPACE_RELATION_ORIENTATION_TRACKED_BIT |
437
373
XRT_SPACE_RELATION_POSITION_VALID_BIT | XRT_SPACE_RELATION_POSITION_TRACKED_BIT);
438
374
Eigen::Vector3f v = pose.translation();
439
-
hand->values.hand_joint_set_default[idx].relation.pose.position.x = -v.x();
440
-
hand->values.hand_joint_set_default[idx].relation.pose.position.y = v.y();
441
-
hand->values.hand_joint_set_default[idx].relation.pose.position.z = v.z();
375
+
hand.values.hand_joint_set_default[idx].relation.pose.position.x = -v.x();
376
+
hand.values.hand_joint_set_default[idx].relation.pose.position.y = v.y();
377
+
hand.values.hand_joint_set_default[idx].relation.pose.position.z = v.z();
442
378
443
379
Eigen::Matrix3f rotation = pose.rotation();
444
380
···
455
391
456
392
q = rotation;
457
393
458
-
hand->values.hand_joint_set_default[idx].relation.pose.orientation.x = q.x();
459
-
hand->values.hand_joint_set_default[idx].relation.pose.orientation.y = q.y();
460
-
hand->values.hand_joint_set_default[idx].relation.pose.orientation.z = q.z();
461
-
hand->values.hand_joint_set_default[idx].relation.pose.orientation.w = q.w();
394
+
hand.values.hand_joint_set_default[idx].relation.pose.orientation.x = q.x();
395
+
hand.values.hand_joint_set_default[idx].relation.pose.orientation.y = q.y();
396
+
hand.values.hand_joint_set_default[idx].relation.pose.orientation.z = q.z();
397
+
hand.values.hand_joint_set_default[idx].relation.pose.orientation.w = q.w();
462
398
}
463
399
464
400
static void
465
-
make_joint_at_matrix(int idx, Eigen::Affine3f &pose, struct xrt_hand_joint_set *hand, int hand_idx)
401
+
make_joint_at_matrix(int idx, Eigen::Affine3f &pose, struct xrt_hand_joint_set &hand, bool is_right)
466
402
{
467
-
if (hand_idx == 0) {
403
+
if (!is_right) {
468
404
make_joint_at_matrix_left_hand(idx, pose, hand);
469
405
} else {
470
406
make_joint_at_matrix_right_hand(idx, pose, hand);
···
473
409
474
410
// Exported:
475
411
void
476
-
optimize_new_frame(xrt_vec3 model_joints_3d[21],
477
-
kinematic_hand_4f *hand,
478
-
struct xrt_hand_joint_set *out_set,
479
-
int hand_idx)
412
+
optimize_new_frame(KinematicHandCCDIK *hand, one_frame_input &observation, struct xrt_hand_joint_set &out_set)
480
413
{
414
+
481
415
// intake poses!
482
416
for (int i = 0; i < 21; i++) {
483
-
if (hand_idx == 0) {
484
-
hand->t_jts[i] = model_joints_3d[i];
417
+
418
+
xrt_vec3 p1 = {0, 0, 0};
419
+
xrt_vec3 p2 = observation.views[0].rays[i];
420
+
421
+
xrt_vec3 p3 = hand->right_in_left.position;
422
+
423
+
xrt_vec3 p4;
424
+
math_quat_rotate_vec3(&hand->right_in_left.orientation, &observation.views[1].rays[i], &p4);
425
+
p4 += hand->right_in_left.position;
426
+
427
+
xrt_vec3 pa;
428
+
xrt_vec3 pb;
429
+
float mua;
430
+
float mub;
431
+
432
+
LineLineIntersect(p1, p2, p3, p4, &pa, &pb, &mua, &mub);
433
+
434
+
xrt_vec3 p;
435
+
p = pa + pb;
436
+
math_vec3_scalar_mul(0.5, &p);
437
+
438
+
439
+
if (!hand->is_right) {
440
+
441
+
hand->t_jts[i] = p;
485
442
} else {
486
-
hand->t_jts[i].x = -model_joints_3d[i].x;
487
-
hand->t_jts[i].y = model_joints_3d[i].y;
488
-
hand->t_jts[i].z = model_joints_3d[i].z;
443
+
hand->t_jts[i].x = -p.x;
444
+
hand->t_jts[i].y = p.y;
445
+
hand->t_jts[i].z = p.z;
489
446
}
490
447
491
448
hand->t_jts_as_mat(0, i) = hand->t_jts[i].x;
···
498
455
499
456
// Convert it to xrt_hand_joint_set!
500
457
501
-
make_joint_at_matrix(XRT_HAND_JOINT_WRIST, hand->wrist_relation, out_set, hand_idx);
458
+
make_joint_at_matrix(XRT_HAND_JOINT_WRIST, hand->wrist_relation, out_set, hand->is_right);
502
459
503
460
Eigen::Affine3f palm_relation;
504
461
···
508
465
palm_relation.translation() += hand->fingers[2].bones[0].world_pose.translation() / 2;
509
466
palm_relation.translation() += hand->fingers[2].bones[1].world_pose.translation() / 2;
510
467
511
-
make_joint_at_matrix(XRT_HAND_JOINT_PALM, palm_relation, out_set, hand_idx);
468
+
make_joint_at_matrix(XRT_HAND_JOINT_PALM, palm_relation, out_set, hand->is_right);
512
469
513
470
int start = XRT_HAND_JOINT_THUMB_METACARPAL;
514
471
···
519
476
520
477
if (!(finger_idx == 0 && bone_idx == 0)) {
521
478
make_joint_at_matrix(start++, hand->fingers[finger_idx].bones[bone_idx].world_pose,
522
-
out_set, hand_idx);
479
+
out_set, hand->is_right);
523
480
}
524
481
}
525
482
}
526
483
527
-
out_set->is_active = true;
484
+
out_set.is_active = true;
528
485
}
529
486
530
487
void
531
-
alloc_kinematic_hand(kinematic_hand_4f **out_kinematic_hand)
488
+
alloc_kinematic_hand(xrt_pose left_in_right, bool is_right, KinematicHandCCDIK **out_kinematic_hand)
532
489
{
533
-
kinematic_hand_4f *h = new kinematic_hand_4f;
490
+
KinematicHandCCDIK *h = new KinematicHandCCDIK();
491
+
h->is_right = is_right;
492
+
493
+
math_pose_invert(&left_in_right, &h->right_in_left);
494
+
495
+
// U_LOG_E("%f %f %f", h->right_in_left.position.x, h->right_in_left.position.y, h->right_in_left.position.z);
496
+
497
+
// Doesn't matter, should get overwritten later.
498
+
init_hardcoded_statics(h, 0.09f);
499
+
534
500
*out_kinematic_hand = h;
535
501
}
536
502
537
503
void
538
-
free_kinematic_hand(kinematic_hand_4f **kinematic_hand)
504
+
free_kinematic_hand(KinematicHandCCDIK **kinematic_hand)
539
505
{
540
506
delete *kinematic_hand;
541
507
}
542
508
543
-
} // namespace xrt::tracking::hand::mercury::kine
509
+
} // namespace xrt::tracking::hand::mercury::ccdik
+6
-4
src/xrt/tracking/hand/mercury/kine/kinematic_tiny_math.hpp
src/xrt/tracking/hand/mercury/kine_ccdik/ccdik_tiny_math.hpp
+6
-4
src/xrt/tracking/hand/mercury/kine/kinematic_tiny_math.hpp
src/xrt/tracking/hand/mercury/kine_ccdik/ccdik_tiny_math.hpp
···
8
8
*/
9
9
10
10
#pragma once
11
-
#include "kinematic_defines.hpp"
11
+
#include "ccdik_defines.hpp"
12
12
13
-
namespace xrt::tracking::hand::mercury::kine {
13
+
namespace xrt::tracking::hand::mercury::ccdik {
14
14
// Waggle-curl-twist.
15
15
static inline void
16
16
wct_to_quat(wct_t wct, struct xrt_quat *out)
···
28
28
xrt_quat just_twist;
29
29
math_quat_from_angle_vector(wct.twist, &twist_axis, &just_twist);
30
30
31
-
//! @todo: optimize This should be a matrix multiplication...
31
+
//! @optimize This should be a matrix multiplication...
32
+
// Are you sure about that, previous moses? Pretty sure that quat products are faster than 3x3 matrix
33
+
// products...
32
34
*out = just_waggle;
33
35
math_quat_rotate(out, &just_curl, out);
34
36
math_quat_rotate(out, &just_twist, out);
···
52
54
{
53
55
clamp(in, c - r, c + r);
54
56
}
55
-
} // namespace xrt::tracking::hand::mercury::kine
57
+
} // namespace xrt::tracking::hand::mercury::ccdik
+20
src/xrt/tracking/hand/mercury/kine_ccdik/CMakeLists.txt
+20
src/xrt/tracking/hand/mercury/kine_ccdik/CMakeLists.txt
···
1
+
# Copyright 2022, Collabora, Ltd.
2
+
# SPDX-License-Identifier: BSL-1.0
3
+
4
+
add_library(
5
+
t_ht_mercury_kine_ccdik STATIC
6
+
ccdik_interface.hpp
7
+
ccdik_main.cpp
8
+
)
9
+
10
+
11
+
target_link_libraries(
12
+
t_ht_mercury_kine_ccdik
13
+
PRIVATE
14
+
aux_math
15
+
aux_tracking
16
+
aux_os
17
+
aux_util
18
+
)
19
+
20
+
target_include_directories(t_ht_mercury_kine_ccdik SYSTEM PRIVATE ${EIGEN3_INCLUDE_DIR})
+31
src/xrt/tracking/hand/mercury/kine_ccdik/ccdik_interface.hpp
+31
src/xrt/tracking/hand/mercury/kine_ccdik/ccdik_interface.hpp
···
1
+
// Copyright 2022, Collabora, Ltd.
2
+
// SPDX-License-Identifier: BSL-1.0
3
+
/*!
4
+
* @file
5
+
* @brief Interface for Cyclic Coordinate Descent IK kinematic optimizer
6
+
* @author Moses Turner <moses@collabora.com>
7
+
* @ingroup tracking
8
+
*/
9
+
#pragma once
10
+
// #include "math/m_api.h"
11
+
12
+
#include "xrt/xrt_defines.h"
13
+
#include "../kine_common.hpp"
14
+
15
+
namespace xrt::tracking::hand::mercury::ccdik {
16
+
17
+
struct KinematicHandCCDIK;
18
+
19
+
void
20
+
alloc_kinematic_hand(xrt_pose left_in_right, bool is_right, KinematicHandCCDIK **out_kinematic_hand);
21
+
22
+
void
23
+
optimize_new_frame(KinematicHandCCDIK *hand, one_frame_input &observation, struct xrt_hand_joint_set &out_set);
24
+
25
+
void
26
+
init_hardcoded_statics(KinematicHandCCDIK *hand, float size = 0.095864);
27
+
28
+
void
29
+
free_kinematic_hand(KinematicHandCCDIK **kinematic_hand);
30
+
31
+
} // namespace xrt::tracking::hand::mercury::ccdik
+89
src/xrt/tracking/hand/mercury/kine_ccdik/lineline.hpp
+89
src/xrt/tracking/hand/mercury/kine_ccdik/lineline.hpp
···
1
+
// Copyright 1998, Paul Bourke.
2
+
// Copyright 2022, Collabora, Ltd.
3
+
// SPDX-License-Identifier: BSL-1.0
4
+
/*!
5
+
* @file
6
+
* @brief Find the closest approach between two lines
7
+
* @author Paul Bourke <paul.bourke@gmail.com>
8
+
* @author Moses Turner <moses@collabora.com>
9
+
* @ingroup tracking
10
+
*/
11
+
#include "float.h"
12
+
#include <limits.h>
13
+
#include <math.h>
14
+
#include "util/u_logging.h"
15
+
#include "xrt/xrt_defines.h"
16
+
#include "stdbool.h"
17
+
18
+
// Taken and relicensed with written permission from http://paulbourke.net/geometry/pointlineplane/lineline.c +
19
+
// http://paulbourke.net/geometry/pointlineplane/
20
+
21
+
/*
22
+
Calculate the line segment PaPb that is the shortest route between
23
+
two lines P1P2 and P3P4. Calculate also the values of mua and mub where
24
+
Pa = P1 + mua (P2 - P1)
25
+
Pb = P3 + mub (P4 - P3)
26
+
Return false if no solution exists.
27
+
*/
28
+
bool
29
+
LineLineIntersect(struct xrt_vec3 p1,
30
+
struct xrt_vec3 p2,
31
+
struct xrt_vec3 p3,
32
+
struct xrt_vec3 p4,
33
+
struct xrt_vec3 *pa,
34
+
struct xrt_vec3 *pb,
35
+
float *mua,
36
+
float *mub)
37
+
{
38
+
struct xrt_vec3 p13, p43, p21; // NOLINT
39
+
float d1343, d4321, d1321, d4343, d2121; // NOLINT
40
+
float number, denom; // NOLINT
41
+
42
+
p13.x = p1.x - p3.x;
43
+
p13.y = p1.y - p3.y;
44
+
p13.z = p1.z - p3.z;
45
+
46
+
p43.x = p4.x - p3.x;
47
+
p43.y = p4.y - p3.y;
48
+
p43.z = p4.z - p3.z;
49
+
// Disabled - just checks that P3P4 isn't length 0, which it won't be.
50
+
#if 0
51
+
if (ABS(p43.x) < FLT_EPSILON && ABS(p43.y) < FLT_EPSILON && ABS(p43.z) < FLT_EPSILON)
52
+
return false;
53
+
#endif
54
+
p21.x = p2.x - p1.x;
55
+
p21.y = p2.y - p1.y;
56
+
p21.z = p2.z - p1.z;
57
+
58
+
// Ditto, checks that P2P1 isn't length 0.
59
+
#if 0
60
+
if (ABS(p21.x) < EPS && ABS(p21.y) < EPS && ABS(p21.z) < EPS)
61
+
return false;
62
+
#endif
63
+
64
+
d1343 = p13.x * p43.x + p13.y * p43.y + p13.z * p43.z;
65
+
d4321 = p43.x * p21.x + p43.y * p21.y + p43.z * p21.z;
66
+
d1321 = p13.x * p21.x + p13.y * p21.y + p13.z * p21.z;
67
+
d4343 = p43.x * p43.x + p43.y * p43.y + p43.z * p43.z;
68
+
d2121 = p21.x * p21.x + p21.y * p21.y + p21.z * p21.z;
69
+
70
+
denom = d2121 * d4343 - d4321 * d4321;
71
+
72
+
// Division-by-zero check
73
+
if (fabsf(denom) < FLT_EPSILON) {
74
+
return false;
75
+
}
76
+
number = d1343 * d4321 - d1321 * d4343;
77
+
78
+
*mua = number / denom;
79
+
*mub = (d1343 + d4321 * (*mua)) / d4343;
80
+
81
+
pa->x = p1.x + *mua * p21.x;
82
+
pa->y = p1.y + *mua * p21.y;
83
+
pa->z = p1.z + *mua * p21.z;
84
+
pb->x = p3.x + *mub * p43.x;
85
+
pb->y = p3.y + *mub * p43.y;
86
+
pb->z = p3.z + *mub * p43.z;
87
+
88
+
return (true);
89
+
}
+67
src/xrt/tracking/hand/mercury/kine_common.hpp
+67
src/xrt/tracking/hand/mercury/kine_common.hpp
···
1
+
// Copyright 2022, Collabora, Ltd.
2
+
// SPDX-License-Identifier: BSL-1.0
3
+
/*!
4
+
* @file
5
+
* @brief Random common stuff for Mercury kinematic optimizers
6
+
* @author Moses Turner <moses@collabora.com>
7
+
* @ingroup tracking
8
+
*/
9
+
10
+
#pragma once
11
+
#include "xrt/xrt_defines.h"
12
+
namespace xrt::tracking::hand::mercury {
13
+
14
+
// Changing this to double should work but you might need to fix some things.
15
+
// Float is faster, and nothing (should) be too big or too small to require double.
16
+
17
+
// Different from `Scalar` in lm_* templates - those can be `Ceres::Jet`s too.
18
+
typedef float HandScalar;
19
+
20
+
21
+
// Inputs to kinematic optimizers
22
+
//!@todo Ask Ryan if adding `= {}` only does something if we do one_frame_one_view bla = {}.
23
+
struct one_frame_one_view
24
+
{
25
+
bool active = true;
26
+
xrt_vec3 rays[21] = {};
27
+
HandScalar confidences[21] = {};
28
+
};
29
+
30
+
struct one_frame_input
31
+
{
32
+
one_frame_one_view views[2] = {};
33
+
};
34
+
35
+
namespace Joint21 {
36
+
enum Joint21
37
+
{
38
+
WRIST = 0,
39
+
40
+
THMB_MCP = 1,
41
+
THMB_PXM = 2,
42
+
THMB_DST = 3,
43
+
THMB_TIP = 4,
44
+
45
+
INDX_PXM = 5,
46
+
INDX_INT = 6,
47
+
INDX_DST = 7,
48
+
INDX_TIP = 8,
49
+
50
+
MIDL_PXM = 9,
51
+
MIDL_INT = 10,
52
+
MIDL_DST = 11,
53
+
MIDL_TIP = 12,
54
+
55
+
RING_PXM = 13,
56
+
RING_INT = 14,
57
+
RING_DST = 15,
58
+
RING_TIP = 16,
59
+
60
+
LITL_PXM = 17,
61
+
LITL_INT = 18,
62
+
LITL_DST = 19,
63
+
LITL_TIP = 20
64
+
};
65
+
}
66
+
67
+
} // namespace xrt::tracking::hand::mercury
+30
src/xrt/tracking/hand/mercury/kine_lm/CMakeLists.txt
+30
src/xrt/tracking/hand/mercury/kine_lm/CMakeLists.txt
···
1
+
# Copyright 2022, Collabora, Ltd.
2
+
# SPDX-License-Identifier: BSL-1.0
3
+
4
+
add_library(t_ht_mercury_kine_lm STATIC lm_interface.hpp lm_main.cpp)
5
+
6
+
target_link_libraries(
7
+
t_ht_mercury_kine_lm
8
+
PRIVATE
9
+
aux_math
10
+
aux_tracking
11
+
aux_os
12
+
aux_util
13
+
xrt-external-tinyceres
14
+
)
15
+
16
+
target_include_directories(t_ht_mercury_kine_lm SYSTEM PRIVATE ${EIGEN3_INCLUDE_DIR})
17
+
18
+
if(${CMAKE_CXX_COMPILER_ID} STREQUAL "Clang")
19
+
target_compile_options(t_ht_mercury_kine_lm PRIVATE -ftemplate-backtrace-limit=20)
20
+
endif()
21
+
22
+
23
+
# Below is entirely just so that tests can find us.
24
+
add_library(
25
+
t_ht_mercury_kine_lm_includes INTERFACE
26
+
)
27
+
28
+
target_include_directories(
29
+
t_ht_mercury_kine_lm_includes INTERFACE ${CMAKE_CURRENT_SOURCE_DIR} ${CMAKE_CURRENT_BINARY_DIR}
30
+
)
+318
src/xrt/tracking/hand/mercury/kine_lm/lm_defines.hpp
+318
src/xrt/tracking/hand/mercury/kine_lm/lm_defines.hpp
···
1
+
// Copyright 2022, Collabora, Ltd.
2
+
// SPDX-License-Identifier: BSL-1.0
3
+
/*!
4
+
* @file
5
+
* @brief Defines for Levenberg-Marquardt kinematic optimizer
6
+
* @author Moses Turner <moses@collabora.com>
7
+
* @ingroup tracking
8
+
*/
9
+
#pragma once
10
+
11
+
// #include <Eigen/Core>
12
+
// #include <Eigen/Geometry>
13
+
#include "math/m_mathinclude.h"
14
+
#include "../kine_common.hpp"
15
+
#include <type_traits>
16
+
17
+
namespace xrt::tracking::hand::mercury::lm {
18
+
19
+
#define LM_TRACE(lmh, ...) U_LOG_IFL_T(lmh.log_level, __VA_ARGS__)
20
+
#define LM_DEBUG(lmh, ...) U_LOG_IFL_D(lmh.log_level, __VA_ARGS__)
21
+
#define LM_INFO(lmh, ...) U_LOG_IFL_I(lmh.log_level, __VA_ARGS__)
22
+
#define LM_WARN(lmh, ...) U_LOG_IFL_W(lmh.log_level, __VA_ARGS__)
23
+
#define LM_ERROR(lmh, ...) U_LOG_IFL_E(lmh.log_level, __VA_ARGS__)
24
+
25
+
// Inlines.
26
+
template <typename T>
27
+
inline T
28
+
rad(T degrees)
29
+
{
30
+
return degrees * T(M_PI / 180.f);
31
+
}
32
+
33
+
// Number of joints that our ML models output.
34
+
static constexpr size_t kNumNNJoints = 21;
35
+
36
+
static constexpr size_t kNumFingers = 5;
37
+
38
+
// This is a lie for the thumb; we usually do the hidden metacarpal trick there
39
+
static constexpr size_t kNumJointsInFinger = 5;
40
+
41
+
static constexpr size_t kNumOrientationsInFinger = 4;
42
+
43
+
// These defines look silly, but they are _extremely_ useful for doing work on this optimizer. Please don't remove them.
44
+
#define USE_HAND_SIZE
45
+
#define USE_HAND_TRANSLATION
46
+
#define USE_HAND_ORIENTATION
47
+
#define USE_EVERYTHING_ELSE
48
+
49
+
// Not tested/tuned well enough; might make tracking slow.
50
+
#undef USE_HAND_PLAUSIBILITY
51
+
52
+
static constexpr size_t kMetacarpalBoneDim = 3;
53
+
static constexpr size_t kProximalBoneDim = 2;
54
+
static constexpr size_t kFingerDim = kProximalBoneDim + kMetacarpalBoneDim + 2;
55
+
static constexpr size_t kThumbDim = kMetacarpalBoneDim + 2;
56
+
static constexpr size_t kHandSizeDim = 1;
57
+
static constexpr size_t kHandTranslationDim = 3;
58
+
static constexpr size_t kHandOrientationDim = 3;
59
+
60
+
61
+
62
+
static constexpr size_t kHRTC_HandSize = 1;
63
+
static constexpr size_t kHRTC_RootBoneTranslation = 3;
64
+
static constexpr size_t kHRTC_RootBoneOrientation = 3; // Direct difference between the two angle-axis rotations. This
65
+
// works well enough because the rotation should be small.
66
+
67
+
static constexpr size_t kHRTC_ThumbMCPSwingTwist = 3;
68
+
static constexpr size_t kHRTC_ThumbCurls = 2;
69
+
70
+
static constexpr size_t kHRTC_ProximalSimilarity = 2;
71
+
72
+
static constexpr size_t kHRTC_FingerMCPSwingTwist = 3;
73
+
static constexpr size_t kHRTC_FingerPXMSwing = 2;
74
+
static constexpr size_t kHRTC_FingerCurls = 2;
75
+
static constexpr size_t kHRTC_CurlSimilarity = 1;
76
+
77
+
static constexpr size_t kHandResidualOneSideSize = 21 * 2;
78
+
79
+
static constexpr size_t kHandResidualTemporalConsistencyOneFingerSize = //
80
+
kHRTC_FingerMCPSwingTwist + //
81
+
kHRTC_FingerPXMSwing + //
82
+
kHRTC_FingerCurls + //
83
+
#ifdef USE_HAND_PLAUSIBILITY //
84
+
kHRTC_CurlSimilarity + //
85
+
#endif //
86
+
0;
87
+
88
+
static constexpr size_t kHandResidualTemporalConsistencySize = //
89
+
kHRTC_RootBoneTranslation + //
90
+
kHRTC_RootBoneOrientation + //
91
+
kHRTC_ThumbMCPSwingTwist + //
92
+
kHRTC_ThumbCurls + //
93
+
#ifdef USE_HAND_PLAUSIBILITY //
94
+
kHRTC_ProximalSimilarity + //
95
+
#endif //
96
+
(kHandResidualTemporalConsistencyOneFingerSize * 4) + //
97
+
0;
98
+
99
+
100
+
// Factors to multiply different values by to get a smooth hand trajectory without introducing too much latency
101
+
102
+
// 1.0 is good, a little jittery.
103
+
// Anything above 3.0 generally breaks.
104
+
static constexpr HandScalar kStabilityRoot = 1.0;
105
+
static constexpr HandScalar kStabilityCurlRoot = kStabilityRoot * 0.03f;
106
+
static constexpr HandScalar kStabilityOtherRoot = kStabilityRoot * 0.03f;
107
+
108
+
static constexpr HandScalar kStabilityThumbMCPSwing = kStabilityCurlRoot * 1.5f;
109
+
static constexpr HandScalar kStabilityThumbMCPTwist = kStabilityCurlRoot * 1.5f;
110
+
111
+
static constexpr HandScalar kStabilityFingerMCPSwing = kStabilityCurlRoot * 3.0f;
112
+
static constexpr HandScalar kStabilityFingerMCPTwist = kStabilityCurlRoot * 3.0f;
113
+
114
+
static constexpr HandScalar kStabilityFingerPXMSwingX = kStabilityCurlRoot * 1.0f;
115
+
static constexpr HandScalar kStabilityFingerPXMSwingY = kStabilityCurlRoot * 1.6f;
116
+
117
+
static constexpr HandScalar kStabilityRootPosition = kStabilityOtherRoot * 30;
118
+
static constexpr HandScalar kStabilityHandSize = kStabilityOtherRoot * 1000;
119
+
120
+
static constexpr HandScalar kStabilityHandOrientation = kStabilityOtherRoot * 3;
121
+
122
+
123
+
static constexpr HandScalar kPlausibilityRoot = 1.0;
124
+
static constexpr HandScalar kPlausibilityProximalSimilarity = 0.05f * kPlausibilityRoot;
125
+
126
+
static constexpr HandScalar kPlausibilityCurlSimilarityHard = 0.10f * kPlausibilityRoot;
127
+
static constexpr HandScalar kPlausibilityCurlSimilaritySoft = 0.05f * kPlausibilityRoot;
128
+
129
+
130
+
constexpr size_t
131
+
calc_input_size(bool optimize_hand_size)
132
+
{
133
+
size_t out = 0;
134
+
135
+
#ifdef USE_HAND_TRANSLATION
136
+
out += kHandTranslationDim;
137
+
#endif
138
+
139
+
#ifdef USE_HAND_ORIENTATION
140
+
out += kHandOrientationDim;
141
+
#endif
142
+
143
+
#ifdef USE_EVERYTHING_ELSE
144
+
out += kThumbDim;
145
+
out += (kFingerDim * 4);
146
+
#endif
147
+
148
+
#ifdef USE_HAND_SIZE
149
+
if (optimize_hand_size) {
150
+
out += kHandSizeDim;
151
+
}
152
+
#endif
153
+
154
+
return out;
155
+
}
156
+
157
+
158
+
constexpr size_t
159
+
calc_residual_size(bool stability, bool optimize_hand_size, int num_views)
160
+
{
161
+
size_t out = 0;
162
+
for (int i = 0; i < num_views; i++) {
163
+
out += kHandResidualOneSideSize;
164
+
}
165
+
166
+
if (stability) {
167
+
out += kHandResidualTemporalConsistencySize;
168
+
}
169
+
170
+
if (optimize_hand_size) {
171
+
out += kHRTC_HandSize;
172
+
}
173
+
174
+
return out;
175
+
}
176
+
177
+
// Some templatable spatial types.
178
+
// Heavily inspired by Eigen - one can definitely use Eigen instead, but here I'd rather have more control
179
+
180
+
template <typename Scalar> struct Quat
181
+
{
182
+
Scalar x;
183
+
Scalar y;
184
+
Scalar z;
185
+
Scalar w;
186
+
187
+
/// Default constructor - DOES NOT INITIALIZE VALUES
188
+
constexpr Quat() {}
189
+
190
+
/// Copy constructor
191
+
constexpr Quat(Quat const &) noexcept(std::is_nothrow_copy_constructible_v<Scalar>) = default;
192
+
193
+
/// Move constructor
194
+
Quat(Quat &&) noexcept(std::is_nothrow_move_constructible_v<Scalar>) = default;
195
+
196
+
/// Copy assignment
197
+
Quat &
198
+
operator=(Quat const &) = default;
199
+
200
+
/// Move assignment
201
+
Quat &
202
+
operator=(Quat &&) noexcept = default;
203
+
204
+
/// Construct from x, y, z, w scalars
205
+
template <typename Other>
206
+
constexpr Quat(Other x, Other y, Other z, Other w) noexcept // NOLINT(bugprone-easily-swappable-parameters)
207
+
: x{Scalar(x)}, y{Scalar(y)}, z{Scalar(z)}, w{Scalar(w)}
208
+
{}
209
+
210
+
/// So that we can copy a regular Vec2 into the real part of a Jet Vec2
211
+
template <typename Other> Quat(Quat<Other> const &other) : Quat(other.x, other.y, other.z, other.w) {}
212
+
213
+
static Quat
214
+
Identity()
215
+
{
216
+
return Quat(0.f, 0.f, 0.f, 1.f);
217
+
}
218
+
};
219
+
220
+
template <typename Scalar> struct Vec3
221
+
{
222
+
// Note that these are not initialized, for performance reasons.
223
+
// If you want them initialized, use Zero() or something else
224
+
Scalar x;
225
+
Scalar y;
226
+
Scalar z;
227
+
228
+
/// Default constructor - DOES NOT INITIALIZE VALUES
229
+
constexpr Vec3() {}
230
+
/// Copy constructor
231
+
constexpr Vec3(Vec3 const &other) noexcept(std::is_nothrow_copy_constructible_v<Scalar>) = default;
232
+
233
+
/// Move constructor
234
+
Vec3(Vec3 &&) noexcept(std::is_nothrow_move_constructible_v<Scalar>) = default;
235
+
236
+
/// Copy assignment
237
+
Vec3 &
238
+
operator=(Vec3 const &) = default;
239
+
240
+
/// Move assignment
241
+
Vec3 &
242
+
operator=(Vec3 &&) noexcept = default;
243
+
244
+
245
+
template <typename Other>
246
+
constexpr Vec3(Other x, Other y, Other z) noexcept // NOLINT(bugprone-easily-swappable-parameters)
247
+
: x{Scalar(x)}, y{Scalar(y)}, z{Scalar(z)}
248
+
{}
249
+
250
+
template <typename Other> Vec3(Vec3<Other> const &other) : Vec3(other.x, other.y, other.z) {}
251
+
252
+
static Vec3
253
+
Zero()
254
+
{
255
+
return Vec3(0.f, 0.f, 0.f);
256
+
}
257
+
};
258
+
259
+
template <typename Scalar> struct Vec2
260
+
{
261
+
Scalar x;
262
+
Scalar y;
263
+
264
+
/// Default constructor - DOES NOT INITIALIZE VALUES
265
+
constexpr Vec2() noexcept {}
266
+
267
+
/// Copy constructor
268
+
constexpr Vec2(Vec2 const &) noexcept(std::is_nothrow_copy_constructible_v<Scalar>) = default;
269
+
270
+
/// Move constructor
271
+
constexpr Vec2(Vec2 &&) noexcept(std::is_nothrow_move_constructible_v<Scalar>) = default;
272
+
273
+
/// Copy assignment
274
+
Vec2 &
275
+
operator=(Vec2 const &) = default;
276
+
277
+
/// Move assignment
278
+
Vec2 &
279
+
operator=(Vec2 &&) noexcept = default;
280
+
281
+
/// So that we can copy a regular Vec2 into the real part of a Jet Vec2
282
+
template <typename Other>
283
+
Vec2(Other x, Other y) // NOLINT(bugprone-easily-swappable-parameters)
284
+
noexcept(std::is_nothrow_constructible_v<Scalar, Other>)
285
+
: x{Scalar(x)}, y{Scalar(y)}
286
+
{}
287
+
288
+
template <typename Other>
289
+
Vec2(Vec2<Other> const &other) noexcept(std::is_nothrow_constructible_v<Scalar, Other>) : Vec2(other.x, other.y)
290
+
{}
291
+
292
+
static constexpr Vec2
293
+
Zero()
294
+
{
295
+
return Vec2(0.f, 0.f);
296
+
}
297
+
};
298
+
299
+
template <typename T> struct ResidualHelper
300
+
{
301
+
T *out_residual;
302
+
size_t out_residual_idx = 0;
303
+
304
+
ResidualHelper(T *residual) : out_residual(residual)
305
+
{
306
+
out_residual_idx = 0;
307
+
}
308
+
309
+
void
310
+
AddValue(T const &value)
311
+
{
312
+
this->out_residual[out_residual_idx++] = value;
313
+
}
314
+
};
315
+
316
+
317
+
318
+
} // namespace xrt::tracking::hand::mercury::lm
+62
src/xrt/tracking/hand/mercury/kine_lm/lm_interface.hpp
+62
src/xrt/tracking/hand/mercury/kine_lm/lm_interface.hpp
···
1
+
// Copyright 2022, Collabora, Ltd.
2
+
// SPDX-License-Identifier: BSL-1.0
3
+
/*!
4
+
* @file
5
+
* @brief Interface for Levenberg-Marquardt kinematic optimizer
6
+
* @author Moses Turner <moses@collabora.com>
7
+
* @ingroup tracking
8
+
*/
9
+
#pragma once
10
+
#include "xrt/xrt_defines.h"
11
+
#include "util/u_logging.h"
12
+
// #include "lm_defines.hpp"
13
+
#include "../kine_common.hpp"
14
+
15
+
namespace xrt::tracking::hand::mercury::lm {
16
+
17
+
// Yes, this is a weird in-between-C-and-C++ API. Fight me, I like it this way.
18
+
19
+
// Opaque struct.
20
+
struct KinematicHandLM;
21
+
22
+
// Constructor
23
+
void
24
+
optimizer_create(xrt_pose left_in_right,
25
+
bool is_right,
26
+
u_logging_level log_level,
27
+
KinematicHandLM **out_kinematic_hand);
28
+
29
+
/*!
30
+
* The main tracking code calls this function with some 2D(ish) camera observations of the hand, and this function
31
+
* calculates a good 3D hand pose and writes it to out_viz_hand.
32
+
*
33
+
* @param observation The observation of the hand joints. Warning, this function will mutate the observation
34
+
* unpredictably. Keep a copy of it if you need it after.
35
+
* @param hand_was_untracked_last_frame: If the hand was untracked last frame (it was out of view, obscured, ML models
36
+
* failed, etc.) - if it was, we don't want to enforce temporal consistency because we have no good previous hand state
37
+
* with which to do that.
38
+
* @param optimize_hand_size: Whether or not it's allowed to tweak the hand size - when we're calibrating the user's
39
+
* hand size, we want to do that; afterwards we don't want to waste the compute.
40
+
* @param target_hand_size: The hand size we want it to get close to
41
+
* @param hand_size_err_mul: A multiplier to help determine how close it has to get to that hand size
42
+
* @param[out] out_hand: The xrt_hand_joint_set to output its result to
43
+
* @param[out] out_hand_size: The hand size it ended up at
44
+
* @param[out] out_reprojection_error: The reprojection error it ended up at
45
+
*/
46
+
47
+
void
48
+
optimizer_run(KinematicHandLM *hand,
49
+
one_frame_input &observation,
50
+
bool hand_was_untracked_last_frame,
51
+
bool optimize_hand_size,
52
+
float target_hand_size,
53
+
float hand_size_err_mul,
54
+
xrt_hand_joint_set &out_hand,
55
+
float &out_hand_size,
56
+
float &out_reprojection_error);
57
+
58
+
// Destructor
59
+
void
60
+
optimizer_destroy(KinematicHandLM **hand);
61
+
62
+
} // namespace xrt::tracking::hand::mercury::lm
+992
src/xrt/tracking/hand/mercury/kine_lm/lm_main.cpp
+992
src/xrt/tracking/hand/mercury/kine_lm/lm_main.cpp
···
1
+
// Copyright 2022, Collabora, Ltd.
2
+
// SPDX-License-Identifier: BSL-1.0
3
+
/*!
4
+
* @file
5
+
* @brief Levenberg-Marquardt kinematic optimizer
6
+
* @author Moses Turner <moses@collabora.com>
7
+
* @author Charlton Rodda <charlton.rodda@collabora.com>
8
+
* @ingroup tracking
9
+
*/
10
+
11
+
#include "math/m_api.h"
12
+
#include "math/m_vec3.h"
13
+
#include "os/os_time.h"
14
+
#include "util/u_logging.h"
15
+
#include "util/u_misc.h"
16
+
#include "util/u_trace_marker.h"
17
+
18
+
#include "tinyceres/tiny_solver.hpp"
19
+
#include "tinyceres/tiny_solver_autodiff_function.hpp"
20
+
#include "lm_rotations.inl"
21
+
22
+
#include <iostream>
23
+
#include <cmath>
24
+
#include <random>
25
+
#include "lm_interface.hpp"
26
+
#include "lm_optimizer_params_packer.inl"
27
+
#include "lm_defines.hpp"
28
+
29
+
/*
30
+
31
+
Some notes:
32
+
Everything templated with <typename T> is basically just a scalar template, usually taking float or ceres::Jet<float, N>
33
+
34
+
*/
35
+
36
+
namespace xrt::tracking::hand::mercury::lm {
37
+
38
+
template <typename T> struct StereographicObservation
39
+
{
40
+
// T obs[kNumNNJoints][2];
41
+
Vec2<T> obs[kNumNNJoints];
42
+
};
43
+
44
+
struct KinematicHandLM
45
+
{
46
+
bool first_frame = true;
47
+
bool use_stability = false;
48
+
bool optimize_hand_size = true;
49
+
bool is_right = false;
50
+
int num_observation_views = 0;
51
+
one_frame_input *observation;
52
+
53
+
HandScalar target_hand_size;
54
+
HandScalar hand_size_err_mul;
55
+
u_logging_level log_level;
56
+
57
+
58
+
StereographicObservation<HandScalar> sgo[2];
59
+
60
+
Quat<HandScalar> last_frame_pre_rotation;
61
+
OptimizerHand<HandScalar> last_frame;
62
+
63
+
// The pose that will take you from the right camera's space to the left camera's space.
64
+
xrt_pose left_in_right;
65
+
66
+
// The translation part of the same pose, just easier for Ceres to consume
67
+
Vec3<HandScalar> left_in_right_translation;
68
+
69
+
// The orientation part of the same pose, just easier for Ceres to consume
70
+
Quat<HandScalar> left_in_right_orientation;
71
+
72
+
Eigen::Matrix<HandScalar, calc_input_size(true), 1> TinyOptimizerInput;
73
+
};
74
+
75
+
template <typename T> struct Translations55
76
+
{
77
+
Vec3<T> t[kNumFingers][kNumJointsInFinger];
78
+
};
79
+
80
+
template <typename T> struct Orientations54
81
+
{
82
+
Quat<T> q[kNumFingers][kNumJointsInFinger];
83
+
};
84
+
85
+
template <bool optimize_hand_size> struct CostFunctor
86
+
{
87
+
KinematicHandLM &parent;
88
+
size_t num_residuals_;
89
+
90
+
template <typename T>
91
+
bool
92
+
operator()(const T *const x, T *residual) const;
93
+
94
+
CostFunctor(KinematicHandLM &in_last_hand, size_t const &num_residuals)
95
+
: parent(in_last_hand), num_residuals_(num_residuals)
96
+
{}
97
+
98
+
size_t
99
+
NumResiduals() const
100
+
{
101
+
return num_residuals_;
102
+
}
103
+
};
104
+
105
+
template <typename T>
106
+
static inline void
107
+
eval_hand_set_rel_translations(const OptimizerHand<T> &opt, Translations55<T> &rel_translations)
108
+
{
109
+
// Basically, we're walking up rel_translations, writing strictly sequentially. Hopefully this is fast.
110
+
111
+
// Thumb metacarpal translation.
112
+
rel_translations.t[0][0] = {(T)0.33097, T(-0.1), (T)-0.25968};
113
+
114
+
// Comes after the invisible joint.
115
+
rel_translations.t[0][1] = {T(0), T(0), T(0)};
116
+
// prox, distal, tip
117
+
rel_translations.t[0][2] = {T(0), T(0), T(-0.389626)};
118
+
rel_translations.t[0][3] = {T(0), T(0), T(-0.311176)};
119
+
rel_translations.t[0][4] = {T(0), T(0), (T)-0.232195};
120
+
121
+
// What's the best place to put this? Here works, but is there somewhere we could put it where it gets accessed
122
+
// faster?
123
+
T finger_joint_lengths[4][4] = {
124
+
{
125
+
T(-0.66),
126
+
T(-0.365719),
127
+
T(-0.231581),
128
+
T(-0.201790),
129
+
},
130
+
{
131
+
T(-0.645),
132
+
T(-0.404486),
133
+
T(-0.247749),
134
+
T(-0.210121),
135
+
},
136
+
{
137
+
T(-0.58),
138
+
T(-0.365639),
139
+
T(-0.225666),
140
+
T(-0.187089),
141
+
},
142
+
{
143
+
T(-0.52),
144
+
T(-0.278197),
145
+
T(-0.176178),
146
+
T(-0.157566),
147
+
},
148
+
};
149
+
150
+
// Index metacarpal
151
+
rel_translations.t[1][0] = {T(0.16926), T(0), T(-0.34437)};
152
+
// Middle
153
+
rel_translations.t[2][0] = {T(0.034639), T(0.01), T(-0.35573)};
154
+
// Ring
155
+
rel_translations.t[3][0] = {T(-0.063625), T(0.005), T(-0.34164)};
156
+
// Little
157
+
rel_translations.t[4][0] = {T(-0.1509), T(-0.005), T(-0.30373)};
158
+
159
+
// Index to little finger
160
+
for (int finger = 0; finger < 4; finger++) {
161
+
for (int i = 0; i < 4; i++) {
162
+
int bone = i + 1;
163
+
rel_translations.t[finger + 1][bone] = {T(0), T(0), T(finger_joint_lengths[finger][i])};
164
+
}
165
+
}
166
+
167
+
// This is done in UnitQuaternionRotateAndScale now.
168
+
// for (int finger = 0; finger < 5; finger++) {
169
+
// for (int bone = 0; bone < 5; bone++) {
170
+
// rel_translations[finger][bone][0] *= opt.hand_size;
171
+
// rel_translations[finger][bone][1] *= opt.hand_size;
172
+
// rel_translations[finger][bone][2] *= opt.hand_size;
173
+
// }
174
+
// }
175
+
}
176
+
177
+
178
+
179
+
template <typename T>
180
+
inline void
181
+
eval_hand_set_rel_orientations(const OptimizerHand<T> &opt, Orientations54<T> &rel_orientations)
182
+
{
183
+
184
+
// Thumb MCP hidden orientation
185
+
#if 0
186
+
Vec2<T> mcp_root_swing;
187
+
188
+
mcp_root_swing.x = rad<T>((T)(-10));
189
+
mcp_root_swing.y = rad<T>((T)(-40));
190
+
191
+
T mcp_root_twist = rad<T>((T)(-80));
192
+
193
+
SwingTwistToQuaternion(mcp_root_swing, mcp_root_twist, rel_orientations.q[0][0]);
194
+
195
+
std::cout << "\n\n\n\nHIDDEN ORIENTATION\n";
196
+
std::cout << std::setprecision(100);
197
+
std::cout << rel_orientations.q[0][0].w << std::endl;
198
+
std::cout << rel_orientations.q[0][0].x << std::endl;
199
+
std::cout << rel_orientations.q[0][0].y << std::endl;
200
+
std::cout << rel_orientations.q[0][0].z << std::endl;
201
+
#else
202
+
// This should be exactly equivalent to the above
203
+
rel_orientations.q[0][0].w = T(0.716990172863006591796875);
204
+
rel_orientations.q[0][0].x = T(0.1541481912136077880859375);
205
+
rel_orientations.q[0][0].y = T(-0.31655871868133544921875);
206
+
rel_orientations.q[0][0].z = T(-0.6016261577606201171875);
207
+
#endif
208
+
209
+
// Thumb MCP orientation
210
+
SwingTwistToQuaternion(opt.thumb.metacarpal.swing, //
211
+
opt.thumb.metacarpal.twist, //
212
+
rel_orientations.q[0][1]);
213
+
214
+
// Thumb curls
215
+
CurlToQuaternion(opt.thumb.rots[0], rel_orientations.q[0][2]);
216
+
CurlToQuaternion(opt.thumb.rots[1], rel_orientations.q[0][3]);
217
+
218
+
// Finger orientations
219
+
for (int i = 0; i < 4; i++) {
220
+
SwingTwistToQuaternion(opt.finger[i].metacarpal.swing, //
221
+
opt.finger[i].metacarpal.twist, //
222
+
rel_orientations.q[i + 1][0]);
223
+
224
+
SwingToQuaternion(opt.finger[i].proximal_swing, //
225
+
rel_orientations.q[i + 1][1]);
226
+
227
+
CurlToQuaternion(opt.finger[i].rots[0], rel_orientations.q[i + 1][2]);
228
+
CurlToQuaternion(opt.finger[i].rots[1], rel_orientations.q[i + 1][3]);
229
+
}
230
+
}
231
+
232
+
233
+
234
+
template <typename T>
235
+
void
236
+
eval_hand_with_orientation(const OptimizerHand<T> &opt,
237
+
bool is_right,
238
+
Translations55<T> &translations_absolute,
239
+
Orientations54<T> &orientations_absolute)
240
+
241
+
{
242
+
XRT_TRACE_MARKER();
243
+
244
+
245
+
Translations55<T> rel_translations; //[kNumFingers][kNumJointsInFinger];
246
+
Orientations54<T> rel_orientations; //[kNumFingers][kNumOrientationsInFinger];
247
+
248
+
eval_hand_set_rel_orientations(opt, rel_orientations);
249
+
250
+
eval_hand_set_rel_translations(opt, rel_translations);
251
+
252
+
Quat<T> orientation_root;
253
+
254
+
Quat<T> post_orientation_quat;
255
+
256
+
AngleAxisToQuaternion(opt.wrist_post_orientation_aax, post_orientation_quat);
257
+
258
+
QuaternionProduct(opt.wrist_pre_orientation_quat, post_orientation_quat, orientation_root);
259
+
260
+
// Get each joint's tracking-relative orientation by rotating its parent-relative orientation by the
261
+
// tracking-relative orientation of its parent.
262
+
for (size_t finger = 0; finger < kNumFingers; finger++) {
263
+
Quat<T> *last_orientation = &orientation_root;
264
+
for (size_t bone = 0; bone < kNumOrientationsInFinger; bone++) {
265
+
Quat<T> &out_orientation = orientations_absolute.q[finger][bone];
266
+
Quat<T> &rel_orientation = rel_orientations.q[finger][bone];
267
+
268
+
QuaternionProduct(*last_orientation, rel_orientation, out_orientation);
269
+
last_orientation = &out_orientation;
270
+
}
271
+
}
272
+
273
+
// Get each joint's tracking-relative position by rotating its parent-relative translation by the
274
+
// tracking-relative orientation of its parent, then adding that to its parent's tracking-relative position.
275
+
for (size_t finger = 0; finger < kNumFingers; finger++) {
276
+
const Vec3<T> *last_translation = &opt.wrist_location;
277
+
const Quat<T> *last_orientation = &orientation_root;
278
+
for (size_t bone = 0; bone < kNumJointsInFinger; bone++) {
279
+
Vec3<T> &out_translation = translations_absolute.t[finger][bone];
280
+
Vec3<T> &rel_translation = rel_translations.t[finger][bone];
281
+
282
+
UnitQuaternionRotateAndScalePoint(*last_orientation, rel_translation, opt.hand_size,
283
+
out_translation);
284
+
285
+
// If this is a right hand, mirror it.
286
+
if (is_right) {
287
+
out_translation.x *= -1;
288
+
}
289
+
290
+
out_translation.x += last_translation->x;
291
+
out_translation.y += last_translation->y;
292
+
out_translation.z += last_translation->z;
293
+
294
+
// Next iteration, the orientation to rotate by should be the tracking-relative orientation of
295
+
// this joint.
296
+
297
+
// If bone < 4 so we don't go over the end of orientations_absolute. I hope this gets optimized
298
+
// out anyway.
299
+
if (bone < 4) {
300
+
last_orientation = &orientations_absolute.q[finger][bone];
301
+
// Ditto for translation
302
+
last_translation = &out_translation;
303
+
}
304
+
}
305
+
}
306
+
}
307
+
308
+
template <typename T>
309
+
void
310
+
computeResidualStability_Finger(const OptimizerFinger<T> &finger,
311
+
const OptimizerFinger<HandScalar> &finger_last,
312
+
ResidualHelper<T> &helper)
313
+
{
314
+
helper.AddValue((finger.metacarpal.swing.x - finger_last.metacarpal.swing.x) * kStabilityFingerMCPSwing);
315
+
316
+
helper.AddValue((finger.metacarpal.swing.y - finger_last.metacarpal.swing.y) * kStabilityFingerMCPSwing);
317
+
318
+
319
+
320
+
helper.AddValue((finger.metacarpal.twist - finger_last.metacarpal.twist) * kStabilityFingerMCPTwist);
321
+
322
+
323
+
324
+
helper.AddValue((finger.proximal_swing.x - finger_last.proximal_swing.x) * kStabilityFingerPXMSwingX);
325
+
helper.AddValue((finger.proximal_swing.y - finger_last.proximal_swing.y) * kStabilityFingerPXMSwingY);
326
+
327
+
helper.AddValue((finger.rots[0] - finger_last.rots[0]) * kStabilityCurlRoot);
328
+
helper.AddValue((finger.rots[1] - finger_last.rots[1]) * kStabilityCurlRoot);
329
+
330
+
#ifdef USE_HAND_PLAUSIBILITY
331
+
if (finger.rots[0] < finger.rots[1]) {
332
+
helper.AddValue((finger.rots[0] - finger.rots[1]) * kPlausibilityCurlSimilarityHard);
333
+
} else {
334
+
helper.AddValue((finger.rots[0] - finger.rots[1]) * kPlausibilityCurlSimilaritySoft);
335
+
}
336
+
#endif
337
+
}
338
+
339
+
template <bool optimize_hand_size, typename T>
340
+
void
341
+
computeResidualStability(const OptimizerHand<T> &hand,
342
+
const OptimizerHand<HandScalar> &last_hand,
343
+
KinematicHandLM &state,
344
+
ResidualHelper<T> &helper)
345
+
{
346
+
347
+
if constexpr (optimize_hand_size) {
348
+
helper.AddValue( //
349
+
(hand.hand_size - state.target_hand_size) * (T)(kStabilityHandSize * state.hand_size_err_mul));
350
+
}
351
+
if (state.first_frame) {
352
+
return;
353
+
}
354
+
355
+
helper.AddValue((last_hand.wrist_location.x - hand.wrist_location.x) * kStabilityRootPosition);
356
+
helper.AddValue((last_hand.wrist_location.y - hand.wrist_location.y) * kStabilityRootPosition);
357
+
helper.AddValue((last_hand.wrist_location.z - hand.wrist_location.z) * kStabilityRootPosition);
358
+
359
+
360
+
helper.AddValue((hand.wrist_post_orientation_aax.x) * (T)(kStabilityHandOrientation));
361
+
helper.AddValue((hand.wrist_post_orientation_aax.y) * (T)(kStabilityHandOrientation));
362
+
helper.AddValue((hand.wrist_post_orientation_aax.z) * (T)(kStabilityHandOrientation));
363
+
364
+
365
+
366
+
helper.AddValue((hand.thumb.metacarpal.swing.x - last_hand.thumb.metacarpal.swing.x) * kStabilityThumbMCPSwing);
367
+
helper.AddValue((hand.thumb.metacarpal.swing.y - last_hand.thumb.metacarpal.swing.y) * kStabilityThumbMCPSwing);
368
+
helper.AddValue((hand.thumb.metacarpal.twist - last_hand.thumb.metacarpal.twist) * kStabilityThumbMCPTwist);
369
+
370
+
helper.AddValue((hand.thumb.rots[0] - last_hand.thumb.rots[0]) * kStabilityCurlRoot);
371
+
helper.AddValue((hand.thumb.rots[1] - last_hand.thumb.rots[1]) * kStabilityCurlRoot);
372
+
#ifdef USE_HAND_PLAUSIBILITY
373
+
helper.AddValue((hand.finger[1].proximal_swing.x - hand.finger[2].proximal_swing.x) *
374
+
kPlausibilityProximalSimilarity);
375
+
helper.AddValue((hand.finger[2].proximal_swing.x - hand.finger[3].proximal_swing.x) *
376
+
kPlausibilityProximalSimilarity);
377
+
#endif
378
+
379
+
380
+
for (int finger_idx = 0; finger_idx < 4; finger_idx++) {
381
+
const OptimizerFinger<HandScalar> &finger_last = last_hand.finger[finger_idx];
382
+
383
+
const OptimizerFinger<T> &finger = hand.finger[finger_idx];
384
+
385
+
computeResidualStability_Finger(finger, finger_last, helper);
386
+
}
387
+
}
388
+
389
+
template <typename T>
390
+
inline void
391
+
normalize_vector_inplace(Vec3<T> &vector)
392
+
{
393
+
T len = (T)(0);
394
+
395
+
len += vector.x * vector.x;
396
+
len += vector.y * vector.y;
397
+
len += vector.z * vector.z;
398
+
399
+
len = sqrt(len);
400
+
401
+
// This is *a* solution ;)
402
+
//!@todo any good template ways to get epsilon for float,double,jet?
403
+
if (len <= FLT_EPSILON) {
404
+
vector.z = T(-1);
405
+
return;
406
+
}
407
+
408
+
vector.x /= len;
409
+
vector.y /= len;
410
+
vector.z /= len;
411
+
}
412
+
413
+
// in size: 3, out size: 2
414
+
template <typename T>
415
+
static inline void
416
+
unit_vector_stereographic_projection(const Vec3<T> &in, Vec2<T> &out)
417
+
{
418
+
out.x = in.x / ((T)1 - in.z);
419
+
out.y = in.y / ((T)1 - in.z);
420
+
}
421
+
422
+
423
+
template <typename T>
424
+
static inline void
425
+
unit_xrt_vec3_stereographic_projection(const xrt_vec3 in, Vec2<T> &out)
426
+
{
427
+
Vec3<T> vec;
428
+
vec.x = (T)(in.x);
429
+
vec.y = (T)(in.y);
430
+
vec.z = (T)(in.z);
431
+
432
+
normalize_vector_inplace(vec);
433
+
434
+
unit_vector_stereographic_projection(vec, out);
435
+
}
436
+
437
+
template <typename T>
438
+
static void
439
+
diff(const Vec3<T> &model_joint_pos,
440
+
const Vec3<T> &move_joint_translation,
441
+
const Quat<T> &move_joint_orientation,
442
+
const StereographicObservation<HandScalar> &observation,
443
+
const HandScalar *confidences,
444
+
const HandScalar amount_we_care,
445
+
int &hand_joint_idx,
446
+
ResidualHelper<T> &helper)
447
+
{
448
+
449
+
Vec3<T> model_joint_dir_rel_camera;
450
+
UnitQuaternionRotatePoint(move_joint_orientation, model_joint_pos, model_joint_dir_rel_camera);
451
+
452
+
model_joint_dir_rel_camera.x = model_joint_dir_rel_camera.x + move_joint_translation.x;
453
+
model_joint_dir_rel_camera.y = model_joint_dir_rel_camera.y + move_joint_translation.y;
454
+
model_joint_dir_rel_camera.z = model_joint_dir_rel_camera.z + move_joint_translation.z;
455
+
456
+
normalize_vector_inplace(model_joint_dir_rel_camera);
457
+
458
+
Vec2<T> stereographic_model_dir;
459
+
unit_vector_stereographic_projection(model_joint_dir_rel_camera, stereographic_model_dir);
460
+
461
+
462
+
const HandScalar confidence = confidences[hand_joint_idx] * amount_we_care;
463
+
const Vec2<T> &observed_ray_sg = observation.obs[hand_joint_idx];
464
+
465
+
helper.AddValue((stereographic_model_dir.x - (T)(observed_ray_sg.x)) * confidence);
466
+
helper.AddValue((stereographic_model_dir.y - (T)(observed_ray_sg.y)) * confidence);
467
+
468
+
hand_joint_idx++;
469
+
}
470
+
471
+
472
+
473
+
template <typename T>
474
+
void
475
+
CostFunctor_PositionsPart(OptimizerHand<T> &hand, KinematicHandLM &state, ResidualHelper<T> &helper)
476
+
{
477
+
478
+
Translations55<T> translations_absolute;
479
+
Orientations54<T> orientations_absolute;
480
+
481
+
HandScalar we_care_joint[] = {1.3, 0.9, 0.9, 1.3};
482
+
HandScalar we_care_finger[] = {1.0, 1.0, 0.8, 0.8, 0.8};
483
+
484
+
eval_hand_with_orientation(hand, state.is_right, translations_absolute, orientations_absolute);
485
+
486
+
for (int view = 0; view < 2; view++) {
487
+
if (!state.observation->views[view].active) {
488
+
continue;
489
+
}
490
+
Vec3<T> move_direction;
491
+
Quat<T> move_orientation;
492
+
493
+
if (view == 0) {
494
+
move_direction = Vec3<T>::Zero();
495
+
move_orientation = Quat<T>::Identity();
496
+
} else {
497
+
move_direction.x = T(state.left_in_right_translation.x);
498
+
move_direction.y = T(state.left_in_right_translation.y);
499
+
move_direction.z = T(state.left_in_right_translation.z);
500
+
501
+
move_orientation.w = T(state.left_in_right_orientation.w);
502
+
move_orientation.x = T(state.left_in_right_orientation.x);
503
+
move_orientation.y = T(state.left_in_right_orientation.y);
504
+
move_orientation.z = T(state.left_in_right_orientation.z);
505
+
}
506
+
int joint_acc_idx = 0;
507
+
508
+
HandScalar *confidences = state.observation->views[view].confidences;
509
+
510
+
diff<T>(hand.wrist_location, move_direction, move_orientation, state.sgo[view], confidences, 1.5,
511
+
joint_acc_idx, helper);
512
+
513
+
for (int finger_idx = 0; finger_idx < 5; finger_idx++) {
514
+
for (int joint_idx = 0; joint_idx < 4; joint_idx++) {
515
+
diff<T>(translations_absolute.t[finger_idx][joint_idx + 1], move_direction,
516
+
move_orientation, state.sgo[view], confidences,
517
+
we_care_finger[finger_idx] * we_care_joint[joint_idx], joint_acc_idx, helper);
518
+
}
519
+
}
520
+
}
521
+
}
522
+
523
+
// template <typename T>
524
+
// void CostFunctor_HandSizePart(OptimizerHand<T> &hand, KinematicHandLM &state, T *residual, size_t &out_residual_idx)
525
+
// {
526
+
527
+
// }
528
+
529
+
template <bool optimize_hand_size>
530
+
template <typename T>
531
+
bool
532
+
CostFunctor<optimize_hand_size>::operator()(const T *const x, T *residual) const
533
+
{
534
+
struct KinematicHandLM &state = this->parent;
535
+
OptimizerHand<T> hand = {};
536
+
// ??? should I do the below? probably.
537
+
Quat<T> tmp = this->parent.last_frame_pre_rotation;
538
+
OptimizerHandInit<T>(hand, tmp);
539
+
OptimizerHandUnpackFromVector(x, state.optimize_hand_size, T(state.target_hand_size), hand);
540
+
541
+
XRT_MAYBE_UNUSED size_t residual_size =
542
+
calc_residual_size(state.use_stability, optimize_hand_size, state.num_observation_views);
543
+
544
+
// 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
545
+
// gradient.
546
+
// But then later this just becomes a waste.
547
+
#if 0
548
+
for (size_t i = 0; i < residual_size; i++) {
549
+
residual[i] = (T)(0);
550
+
}
551
+
#endif
552
+
553
+
ResidualHelper<T> helper(residual);
554
+
555
+
CostFunctor_PositionsPart(hand, state, helper);
556
+
computeResidualStability<optimize_hand_size, T>(hand, state.last_frame, state, helper);
557
+
558
+
// Bounds checking - we should have written exactly to the end.
559
+
// U_LOG_E("%zu %zu", helper.out_residual_idx, residual_size);
560
+
assert(helper.out_residual_idx == residual_size);
561
+
// If you're hacking, feel free to turn this off; just remember to not write off the end, and to initialize
562
+
// everything somewhere (maybe change the above to an #if 1? )
563
+
564
+
return true;
565
+
}
566
+
567
+
568
+
template <typename T>
569
+
static inline void
570
+
zldtt_ori_right(Quat<T> &orientation, xrt_quat *out)
571
+
{
572
+
struct xrt_quat tmp;
573
+
tmp.w = orientation.w;
574
+
tmp.x = orientation.x;
575
+
tmp.y = orientation.y;
576
+
tmp.z = orientation.z;
577
+
578
+
xrt_vec3 x = XRT_VEC3_UNIT_X;
579
+
xrt_vec3 z = XRT_VEC3_UNIT_Z;
580
+
581
+
math_quat_rotate_vec3(&tmp, &x, &x);
582
+
math_quat_rotate_vec3(&tmp, &z, &z);
583
+
584
+
// This is a very squashed change-of-basis from left-handed coordinate systems to right-handed coordinate
585
+
// systems: you multiply everything by (-1 0 0) then negate the X axis.
586
+
587
+
x.y *= -1;
588
+
x.z *= -1;
589
+
590
+
z.x *= -1;
591
+
592
+
math_quat_from_plus_x_z(&x, &z, out);
593
+
}
594
+
595
+
template <typename T>
596
+
static inline void
597
+
zldtt_ori_left(Quat<T> &orientation, xrt_quat *out)
598
+
{
599
+
out->w = orientation.w;
600
+
out->x = orientation.x;
601
+
out->y = orientation.y;
602
+
out->z = orientation.z;
603
+
}
604
+
605
+
template <typename T>
606
+
static inline void
607
+
zldtt(Vec3<T> &trans, Quat<T> &orientation, bool is_right, xrt_space_relation &out)
608
+
{
609
+
610
+
out.relation_flags = (enum xrt_space_relation_flags)(
611
+
XRT_SPACE_RELATION_ORIENTATION_VALID_BIT | XRT_SPACE_RELATION_ORIENTATION_TRACKED_BIT |
612
+
XRT_SPACE_RELATION_POSITION_VALID_BIT | XRT_SPACE_RELATION_POSITION_TRACKED_BIT);
613
+
out.pose.position.x = trans.x;
614
+
out.pose.position.y = trans.y;
615
+
out.pose.position.z = trans.z;
616
+
if (is_right) {
617
+
zldtt_ori_right(orientation, &out.pose.orientation);
618
+
} else {
619
+
zldtt_ori_left(orientation, &out.pose.orientation);
620
+
}
621
+
}
622
+
623
+
static void
624
+
eval_to_viz_hand(KinematicHandLM &state, xrt_hand_joint_set &out_viz_hand)
625
+
{
626
+
XRT_TRACE_MARKER();
627
+
628
+
//!@todo It's _probably_ fine to have the bigger size?
629
+
Eigen::Matrix<HandScalar, calc_input_size(true), 1> pose = state.TinyOptimizerInput.cast<HandScalar>();
630
+
631
+
OptimizerHand<HandScalar> opt = {};
632
+
OptimizerHandInit(opt, state.last_frame_pre_rotation);
633
+
OptimizerHandUnpackFromVector(pose.data(), state.optimize_hand_size, state.target_hand_size, opt);
634
+
635
+
Translations55<HandScalar> translations_absolute;
636
+
Orientations54<HandScalar> orientations_absolute;
637
+
// Vec3<HandScalar> translations_absolute[kNumFingers][kNumJointsInFinger];
638
+
// Quat<HandScalar> orientations_absolute[kNumFingers][kNumOrientationsInFinger];
639
+
640
+
eval_hand_with_orientation(opt, state.is_right, translations_absolute, orientations_absolute);
641
+
642
+
Quat<HandScalar> post_wrist_orientation;
643
+
644
+
AngleAxisToQuaternion(opt.wrist_post_orientation_aax, post_wrist_orientation);
645
+
646
+
Quat<HandScalar> pre_wrist_orientation = state.last_frame_pre_rotation;
647
+
648
+
// for (int i = 0; i < 4; i++) {
649
+
// pre_wrist_orientation[i] = state.last_frame_pre_rotation[i];
650
+
// }
651
+
652
+
Quat<HandScalar> final_wrist_orientation;
653
+
654
+
QuaternionProduct(pre_wrist_orientation, post_wrist_orientation, final_wrist_orientation);
655
+
656
+
int joint_acc_idx = 0;
657
+
658
+
// Palm.
659
+
660
+
Vec3<HandScalar> palm_position;
661
+
palm_position.x = (translations_absolute.t[2][0].x + translations_absolute.t[2][1].x) / 2;
662
+
palm_position.y = (translations_absolute.t[2][0].y + translations_absolute.t[2][1].y) / 2;
663
+
palm_position.z = (translations_absolute.t[2][0].z + translations_absolute.t[2][1].z) / 2;
664
+
665
+
Quat<HandScalar> &palm_orientation = orientations_absolute.q[2][0];
666
+
667
+
zldtt(palm_position, palm_orientation, state.is_right,
668
+
out_viz_hand.values.hand_joint_set_default[joint_acc_idx++].relation);
669
+
670
+
// Wrist.
671
+
zldtt(opt.wrist_location, final_wrist_orientation, state.is_right,
672
+
out_viz_hand.values.hand_joint_set_default[joint_acc_idx++].relation);
673
+
674
+
for (int finger = 0; finger < 5; finger++) {
675
+
for (int joint = 0; joint < 5; joint++) {
676
+
// This one is necessary
677
+
if (finger == 0 && joint == 0) {
678
+
continue;
679
+
}
680
+
Quat<HandScalar> *orientation;
681
+
if (joint != 4) {
682
+
orientation = &orientations_absolute.q[finger][joint];
683
+
} else {
684
+
orientation = &orientations_absolute.q[finger][joint - 1];
685
+
}
686
+
zldtt(translations_absolute.t[finger][joint], *orientation, state.is_right,
687
+
out_viz_hand.values.hand_joint_set_default[joint_acc_idx++].relation);
688
+
}
689
+
}
690
+
out_viz_hand.is_active = true;
691
+
}
692
+
693
+
//!@todo
694
+
// This reprojection error thing is probably the wrong way of doing it.
695
+
// I think that the right way is to hack (?) TinySolver such that we get the last set of residuals back, and use those
696
+
// somehow. Maybe scale them simply by the spread of the input hand rays? What about handling stereographic projections?
697
+
// worth it? The main bit is that we should just use the last set of residuals somehow, calculating all this is a waste
698
+
// of cycles when we have something that already should work.
699
+
700
+
static void
701
+
repro_error_make_joint_ray(const xrt_hand_joint_value &joint, const xrt_pose &cam_pose, xrt_vec3 &out_ray)
702
+
{
703
+
// by VALUE
704
+
xrt_vec3 position = joint.relation.pose.position;
705
+
706
+
math_quat_rotate_vec3(&cam_pose.orientation, &position, &position);
707
+
position = position + cam_pose.position;
708
+
709
+
out_ray = m_vec3_normalize(position);
710
+
}
711
+
712
+
static enum xrt_hand_joint
713
+
joint_from_21(int finger, int joint)
714
+
{
715
+
if (finger > 0) {
716
+
return xrt_hand_joint(2 + (finger * 5) + joint);
717
+
} else {
718
+
return xrt_hand_joint(joint + 3);
719
+
}
720
+
}
721
+
722
+
static inline float
723
+
simple_vec3_difference(const xrt_vec3 one, const xrt_vec3 two)
724
+
{
725
+
return (1.0 - m_vec3_dot(one, two));
726
+
}
727
+
728
+
float
729
+
reprojection_error_2d(KinematicHandLM &state, const one_frame_input &observation, const xrt_hand_joint_set &set)
730
+
{
731
+
float final_err = 0;
732
+
int views_looked_at = 0;
733
+
for (int view = 0; view < 2; view++) {
734
+
if (!observation.views[view].active) {
735
+
continue;
736
+
}
737
+
views_looked_at++;
738
+
xrt_pose move_amount;
739
+
740
+
if (view == 0) {
741
+
// left camera.
742
+
move_amount = XRT_POSE_IDENTITY;
743
+
} else {
744
+
move_amount = state.left_in_right;
745
+
}
746
+
747
+
xrt_vec3 lm_rays[21];
748
+
const xrt_vec3 *obs_rays = observation.views[view].rays;
749
+
750
+
751
+
int acc_idx = 0;
752
+
753
+
repro_error_make_joint_ray(set.values.hand_joint_set_default[XRT_HAND_JOINT_WRIST], move_amount,
754
+
lm_rays[acc_idx++]);
755
+
756
+
for (int finger = 0; finger < 5; finger++) {
757
+
for (int joint = 0; joint < 4; joint++) {
758
+
repro_error_make_joint_ray(
759
+
set.values.hand_joint_set_default[joint_from_21(finger, joint)], move_amount,
760
+
lm_rays[acc_idx++]);
761
+
}
762
+
}
763
+
764
+
xrt_vec3 obs_center = {};
765
+
xrt_vec3 lm_center = {};
766
+
767
+
float err = 0;
768
+
float obs_spread = 0;
769
+
float lm_spread = 0;
770
+
771
+
for (int i = 0; i < 21; i++) {
772
+
obs_center += obs_rays[i];
773
+
lm_center += lm_rays[i];
774
+
err += simple_vec3_difference(lm_rays[i], obs_rays[i]);
775
+
}
776
+
777
+
math_vec3_scalar_mul(1.0f / 21.0f, &obs_center);
778
+
math_vec3_scalar_mul(1.0f / 21.0f, &lm_center);
779
+
780
+
for (int i = 0; i < 21; i++) {
781
+
obs_spread += simple_vec3_difference(obs_center, obs_rays[i]);
782
+
}
783
+
for (int i = 0; i < 21; i++) {
784
+
lm_spread += simple_vec3_difference(lm_center, lm_rays[i]);
785
+
}
786
+
787
+
788
+
789
+
// std::cout << err << std::endl;
790
+
// std::cout << err / (obs_spread + lm_spread) << std::endl;
791
+
792
+
// return ;
793
+
final_err += err / (obs_spread + lm_spread);
794
+
}
795
+
return final_err / (float)views_looked_at;
796
+
}
797
+
798
+
template <bool optimize_hand_size>
799
+
inline float
800
+
opt_run(KinematicHandLM &state, one_frame_input &observation, xrt_hand_joint_set &out_viz_hand)
801
+
{
802
+
constexpr size_t input_size = calc_input_size(optimize_hand_size);
803
+
804
+
size_t residual_size = calc_residual_size(state.use_stability, optimize_hand_size, state.num_observation_views);
805
+
806
+
LM_DEBUG(state, "Running with %zu inputs and %zu residuals, viewed in %d cameras", input_size, residual_size,
807
+
state.num_observation_views);
808
+
809
+
CostFunctor<optimize_hand_size> cf(state, residual_size);
810
+
811
+
using AutoDiffCostFunctor =
812
+
ceres::TinySolverAutoDiffFunction<CostFunctor<optimize_hand_size>, Eigen::Dynamic, input_size, HandScalar>;
813
+
814
+
AutoDiffCostFunctor f(cf);
815
+
816
+
817
+
// Okay I have no idea if this should be {}-initialized or not. Previous me seems to have thought no, but it
818
+
// works either way.
819
+
ceres::TinySolver<AutoDiffCostFunctor> solver = {};
820
+
solver.options.max_num_iterations = 50;
821
+
// We need to do a parameter sweep for the trust region and see what's fastest.
822
+
// solver.options.initial_trust_region_radius = 1e3;
823
+
solver.options.function_tolerance = 1e-6;
824
+
825
+
Eigen::Matrix<HandScalar, input_size, 1> inp = state.TinyOptimizerInput.head<input_size>();
826
+
827
+
XRT_MAYBE_UNUSED uint64_t start = os_monotonic_get_ns();
828
+
XRT_MAYBE_UNUSED auto summary = solver.Solve(f, &inp);
829
+
XRT_MAYBE_UNUSED uint64_t end = os_monotonic_get_ns();
830
+
831
+
//!@todo Is there a zero-copy way of doing this?
832
+
state.TinyOptimizerInput.head<input_size>() = inp;
833
+
834
+
if (state.log_level <= U_LOGGING_DEBUG) {
835
+
836
+
uint64_t diff = end - start;
837
+
double time_taken = (double)diff / (double)U_TIME_1MS_IN_NS;
838
+
839
+
const char *status;
840
+
841
+
switch (summary.status) {
842
+
case 0: {
843
+
status = "GRADIENT_TOO_SMALL";
844
+
} break;
845
+
case 1: {
846
+
status = "RELATIVE_STEP_SIZE_TOO_SMALL";
847
+
} break;
848
+
case 2: {
849
+
status = "COST_TOO_SMALL";
850
+
} break;
851
+
case 3: {
852
+
status = "HIT_MAX_ITERATIONS";
853
+
} break;
854
+
case 4: {
855
+
status = "COST_CHANGE_TOO_SMALL";
856
+
} break;
857
+
}
858
+
859
+
LM_DEBUG(state, "Status: %s, num_iterations %d, max_norm %E, gtol %E", status, summary.iterations,
860
+
summary.gradient_max_norm, solver.options.gradient_tolerance);
861
+
LM_DEBUG(state, "Took %f ms", time_taken);
862
+
if (summary.iterations < 3) {
863
+
LM_DEBUG(state, "Suspiciouisly low number of iterations!");
864
+
}
865
+
}
866
+
return 0;
867
+
}
868
+
869
+
870
+
void
871
+
hand_was_untracked(KinematicHandLM *hand)
872
+
{
873
+
hand->first_frame = true;
874
+
hand->last_frame_pre_rotation.w = 1.0;
875
+
hand->last_frame_pre_rotation.x = 0.0;
876
+
hand->last_frame_pre_rotation.y = 0.0;
877
+
hand->last_frame_pre_rotation.z = 0.0;
878
+
879
+
OptimizerHandInit(hand->last_frame, hand->last_frame_pre_rotation);
880
+
OptimizerHandPackIntoVector(hand->last_frame, hand->optimize_hand_size, hand->TinyOptimizerInput.data());
881
+
}
882
+
883
+
void
884
+
optimizer_run(KinematicHandLM *hand,
885
+
one_frame_input &observation,
886
+
bool hand_was_untracked_last_frame,
887
+
bool optimize_hand_size,
888
+
float target_hand_size,
889
+
float hand_size_err_mul,
890
+
xrt_hand_joint_set &out_viz_hand,
891
+
float &out_hand_size,
892
+
float &out_reprojection_error) // NOLINT(bugprone-easily-swappable-parameters)
893
+
{
894
+
KinematicHandLM &state = *hand;
895
+
896
+
if (hand_was_untracked_last_frame) {
897
+
hand_was_untracked(hand);
898
+
}
899
+
900
+
state.num_observation_views = 0;
901
+
for (int i = 0; i < 2; i++) {
902
+
if (observation.views[i].active) {
903
+
state.num_observation_views++;
904
+
}
905
+
}
906
+
907
+
state.optimize_hand_size = optimize_hand_size;
908
+
state.target_hand_size = target_hand_size;
909
+
state.hand_size_err_mul = hand_size_err_mul;
910
+
911
+
state.use_stability = !state.first_frame;
912
+
913
+
state.observation = &observation;
914
+
915
+
for (int i = 0; i < 21; i++) {
916
+
for (int view = 0; view < 2; view++) {
917
+
unit_xrt_vec3_stereographic_projection(observation.views[view].rays[i], state.sgo[view].obs[i]);
918
+
}
919
+
}
920
+
921
+
922
+
// For now, we have to statically instantiate different versions of the optimizer depending on how many input
923
+
// parameters there are. For now, there are only two cases - either we are optimizing the hand size or we are
924
+
// not optimizing it.
925
+
// !@todo Can we make a magic template that automatically instantiates the right one, and also make it so we can
926
+
// decide to either make the residual size dynamic or static? Currently, it's dynamic, which is easier for us
927
+
// and makes compile times a lot lower, but it probably makes things some amount slower at runtime.
928
+
929
+
if (optimize_hand_size) {
930
+
opt_run<true>(state, observation, out_viz_hand);
931
+
} else {
932
+
opt_run<false>(state, observation, out_viz_hand);
933
+
}
934
+
935
+
936
+
937
+
// Postfix - unpack,
938
+
OptimizerHandUnpackFromVector(state.TinyOptimizerInput.data(), state.optimize_hand_size, state.target_hand_size,
939
+
state.last_frame);
940
+
941
+
942
+
943
+
// Squash the orientations
944
+
OptimizerHandSquashRotations(state.last_frame, state.last_frame_pre_rotation);
945
+
946
+
// Repack - brings the curl values back into original domain. Look at ModelToLM/LMToModel, we're using sin/asin.
947
+
OptimizerHandPackIntoVector(state.last_frame, hand->optimize_hand_size, state.TinyOptimizerInput.data());
948
+
949
+
950
+
951
+
eval_to_viz_hand(state, out_viz_hand);
952
+
953
+
state.first_frame = false;
954
+
955
+
out_hand_size = state.last_frame.hand_size;
956
+
957
+
out_reprojection_error = reprojection_error_2d(state, observation, out_viz_hand);
958
+
}
959
+
960
+
961
+
962
+
void
963
+
optimizer_create(xrt_pose left_in_right, bool is_right, u_logging_level log_level, KinematicHandLM **out_kinematic_hand)
964
+
{
965
+
KinematicHandLM *hand = new KinematicHandLM;
966
+
967
+
hand->is_right = is_right;
968
+
hand->left_in_right = left_in_right;
969
+
hand->log_level = log_level;
970
+
971
+
hand->left_in_right_translation.x = left_in_right.position.x;
972
+
hand->left_in_right_translation.y = left_in_right.position.y;
973
+
hand->left_in_right_translation.z = left_in_right.position.z;
974
+
975
+
hand->left_in_right_orientation.w = left_in_right.orientation.w;
976
+
hand->left_in_right_orientation.x = left_in_right.orientation.x;
977
+
hand->left_in_right_orientation.y = left_in_right.orientation.y;
978
+
hand->left_in_right_orientation.z = left_in_right.orientation.z;
979
+
980
+
// Probably unnecessary.
981
+
hand_was_untracked(hand);
982
+
983
+
*out_kinematic_hand = hand;
984
+
}
985
+
986
+
void
987
+
optimizer_destroy(KinematicHandLM **hand)
988
+
{
989
+
delete *hand;
990
+
hand = NULL;
991
+
}
992
+
} // namespace xrt::tracking::hand::mercury::lm
+334
src/xrt/tracking/hand/mercury/kine_lm/lm_optimizer_params_packer.inl
+334
src/xrt/tracking/hand/mercury/kine_lm/lm_optimizer_params_packer.inl
···
1
+
// Copyright 2022, Collabora, Ltd.
2
+
// SPDX-License-Identifier: BSL-1.0
3
+
/*!
4
+
* @file
5
+
* @brief Util to reinterpret Ceres parameter vectors as hand model parameters
6
+
* @author Moses Turner <moses@collabora.com>
7
+
* @author Charlton Rodda <charlton.rodda@collabora.com>
8
+
* @ingroup tracking
9
+
*/
10
+
11
+
#include "util/u_logging.h"
12
+
#include "math/m_api.h"
13
+
14
+
#include "lm_interface.hpp"
15
+
#include "lm_defines.hpp"
16
+
#include <cmath>
17
+
#include <iostream>
18
+
19
+
// #include "lm_rotations.hpp"
20
+
21
+
namespace xrt::tracking::hand::mercury::lm {
22
+
23
+
template <typename T> struct OptimizerMetacarpalBone
24
+
{
25
+
Vec2<T> swing;
26
+
T twist;
27
+
};
28
+
29
+
template <typename T> struct OptimizerFinger
30
+
{
31
+
OptimizerMetacarpalBone<T> metacarpal;
32
+
Vec2<T> proximal_swing;
33
+
// Not Vec2.
34
+
T rots[2];
35
+
};
36
+
37
+
template <typename T> struct OptimizerThumb
38
+
{
39
+
OptimizerMetacarpalBone<T> metacarpal;
40
+
// Again not Vec2.
41
+
T rots[2];
42
+
};
43
+
44
+
template <typename T> struct OptimizerHand
45
+
{
46
+
T hand_size;
47
+
Vec3<T> wrist_location;
48
+
// This is constant, a ceres::Rotation.h quat,, taken from last frame.
49
+
Quat<T> wrist_pre_orientation_quat;
50
+
// This is optimized - angle-axis rotation vector. Starts at 0, loss goes up the higher it goes because it
51
+
// indicates more of a rotation.
52
+
Vec3<T> wrist_post_orientation_aax;
53
+
OptimizerThumb<T> thumb = {};
54
+
OptimizerFinger<T> finger[4] = {};
55
+
};
56
+
57
+
58
+
struct minmax
59
+
{
60
+
HandScalar min = 0;
61
+
HandScalar max = 0;
62
+
};
63
+
64
+
class FingerLimit
65
+
{
66
+
public:
67
+
minmax mcp_swing_x = {};
68
+
minmax mcp_swing_y = {};
69
+
minmax mcp_twist = {};
70
+
71
+
minmax pxm_swing_x = {};
72
+
minmax pxm_swing_y = {};
73
+
74
+
minmax curls[2] = {}; // int, dst
75
+
};
76
+
77
+
class HandLimit
78
+
{
79
+
public:
80
+
minmax hand_size;
81
+
82
+
minmax thumb_mcp_swing_x, thumb_mcp_swing_y, thumb_mcp_twist;
83
+
minmax thumb_curls[2];
84
+
85
+
FingerLimit fingers[4];
86
+
87
+
HandLimit()
88
+
{
89
+
hand_size = {0.095 - 0.03, 0.095 + 0.03};
90
+
91
+
thumb_mcp_swing_x = {rad<HandScalar>(-60), rad<HandScalar>(60)};
92
+
thumb_mcp_swing_y = {rad<HandScalar>(-60), rad<HandScalar>(60)};
93
+
thumb_mcp_twist = {rad<HandScalar>(-35), rad<HandScalar>(35)};
94
+
95
+
for (int i = 0; i < 2; i++) {
96
+
thumb_curls[i] = {rad<HandScalar>(-90), rad<HandScalar>(40)};
97
+
}
98
+
99
+
100
+
constexpr double margin = 0.09;
101
+
102
+
fingers[0].mcp_swing_y = {-0.19 - margin, -0.19 + margin};
103
+
fingers[1].mcp_swing_y = {0.00 - margin, 0.00 + margin};
104
+
fingers[2].mcp_swing_y = {0.19 - margin, 0.19 + margin};
105
+
fingers[3].mcp_swing_y = {0.38 - margin, 0.38 + margin};
106
+
107
+
108
+
for (int finger_idx = 0; finger_idx < 4; finger_idx++) {
109
+
FingerLimit &finger = fingers[finger_idx];
110
+
111
+
finger.mcp_swing_x = {rad<HandScalar>(-10), rad<HandScalar>(10)};
112
+
finger.mcp_twist = {rad<HandScalar>(-4), rad<HandScalar>(4)};
113
+
114
+
finger.pxm_swing_x = {rad<HandScalar>(-100), rad<HandScalar>(20)}; // ??? why is it reversed
115
+
finger.pxm_swing_y = {rad<HandScalar>(-20), rad<HandScalar>(20)};
116
+
117
+
for (int i = 0; i < 2; i++) {
118
+
finger.curls[i] = {rad<HandScalar>(-90), rad<HandScalar>(10)};
119
+
}
120
+
}
121
+
}
122
+
};
123
+
124
+
static const class HandLimit the_limit = {};
125
+
126
+
127
+
constexpr HandScalar hand_size_min = 0.095 - 0.03;
128
+
constexpr HandScalar hand_size_max = 0.095 + 0.03;
129
+
130
+
template <typename T>
131
+
inline T
132
+
LMToModel(T lm, minmax mm)
133
+
{
134
+
return mm.min + ((sin(lm) + T(1)) * ((mm.max - mm.min) * T(.5)));
135
+
}
136
+
137
+
template <typename T>
138
+
inline T
139
+
ModelToLM(T model, minmax mm)
140
+
{
141
+
return asin((2 * (model - mm.min) / (mm.max - mm.min)) - 1);
142
+
}
143
+
144
+
// Input vector,
145
+
template <typename T>
146
+
void
147
+
OptimizerHandUnpackFromVector(const T *in, bool use_hand_size, T hand_size, OptimizerHand<T> &out)
148
+
{
149
+
150
+
size_t acc_idx = 0;
151
+
#ifdef USE_HAND_TRANSLATION
152
+
out.wrist_location.x = in[acc_idx++];
153
+
out.wrist_location.y = in[acc_idx++];
154
+
out.wrist_location.z = in[acc_idx++];
155
+
#endif
156
+
#ifdef USE_HAND_ORIENTATION
157
+
out.wrist_post_orientation_aax.x = in[acc_idx++];
158
+
out.wrist_post_orientation_aax.y = in[acc_idx++];
159
+
out.wrist_post_orientation_aax.z = in[acc_idx++];
160
+
#endif
161
+
162
+
#ifdef USE_EVERYTHING_ELSE
163
+
164
+
out.thumb.metacarpal.swing.x = LMToModel(in[acc_idx++], the_limit.thumb_mcp_swing_x);
165
+
out.thumb.metacarpal.swing.y = LMToModel(in[acc_idx++], the_limit.thumb_mcp_swing_y);
166
+
out.thumb.metacarpal.twist = LMToModel(in[acc_idx++], the_limit.thumb_mcp_twist);
167
+
168
+
out.thumb.rots[0] = LMToModel(in[acc_idx++], the_limit.thumb_curls[0]);
169
+
out.thumb.rots[1] = LMToModel(in[acc_idx++], the_limit.thumb_curls[1]);
170
+
171
+
for (int finger_idx = 0; finger_idx < 4; finger_idx++) {
172
+
173
+
out.finger[finger_idx].metacarpal.swing.x =
174
+
LMToModel(in[acc_idx++], the_limit.fingers[finger_idx].mcp_swing_x);
175
+
176
+
out.finger[finger_idx].metacarpal.swing.y =
177
+
LMToModel(in[acc_idx++], the_limit.fingers[finger_idx].mcp_swing_y);
178
+
179
+
out.finger[finger_idx].metacarpal.twist =
180
+
LMToModel(in[acc_idx++], the_limit.fingers[finger_idx].mcp_twist);
181
+
182
+
183
+
out.finger[finger_idx].proximal_swing.x =
184
+
LMToModel(in[acc_idx++], the_limit.fingers[finger_idx].pxm_swing_x);
185
+
out.finger[finger_idx].proximal_swing.y =
186
+
LMToModel(in[acc_idx++], the_limit.fingers[finger_idx].pxm_swing_y);
187
+
188
+
out.finger[finger_idx].rots[0] = LMToModel(in[acc_idx++], the_limit.fingers[finger_idx].curls[0]);
189
+
out.finger[finger_idx].rots[1] = LMToModel(in[acc_idx++], the_limit.fingers[finger_idx].curls[1]);
190
+
}
191
+
#endif
192
+
193
+
#ifdef USE_HAND_SIZE
194
+
if (use_hand_size) {
195
+
out.hand_size = LMToModel(in[acc_idx++], the_limit.hand_size);
196
+
} else {
197
+
out.hand_size = hand_size;
198
+
}
199
+
#endif
200
+
}
201
+
202
+
template <typename T>
203
+
void
204
+
OptimizerHandPackIntoVector(OptimizerHand<T> &in, bool use_hand_size, T *out)
205
+
{
206
+
size_t acc_idx = 0;
207
+
208
+
#ifdef USE_HAND_TRANSLATION
209
+
out[acc_idx++] = in.wrist_location.x;
210
+
out[acc_idx++] = in.wrist_location.y;
211
+
out[acc_idx++] = in.wrist_location.z;
212
+
#endif
213
+
#ifdef USE_HAND_ORIENTATION
214
+
out[acc_idx++] = in.wrist_post_orientation_aax.x;
215
+
out[acc_idx++] = in.wrist_post_orientation_aax.y;
216
+
out[acc_idx++] = in.wrist_post_orientation_aax.z;
217
+
#endif
218
+
#ifdef USE_EVERYTHING_ELSE
219
+
out[acc_idx++] = ModelToLM(in.thumb.metacarpal.swing.x, the_limit.thumb_mcp_swing_x);
220
+
out[acc_idx++] = ModelToLM(in.thumb.metacarpal.swing.y, the_limit.thumb_mcp_swing_y);
221
+
out[acc_idx++] = ModelToLM(in.thumb.metacarpal.twist, the_limit.thumb_mcp_twist);
222
+
223
+
out[acc_idx++] = ModelToLM(in.thumb.rots[0], the_limit.thumb_curls[0]);
224
+
out[acc_idx++] = ModelToLM(in.thumb.rots[1], the_limit.thumb_curls[1]);
225
+
226
+
for (int finger_idx = 0; finger_idx < 4; finger_idx++) {
227
+
out[acc_idx++] =
228
+
ModelToLM(in.finger[finger_idx].metacarpal.swing.x, the_limit.fingers[finger_idx].mcp_swing_x);
229
+
out[acc_idx++] =
230
+
ModelToLM(in.finger[finger_idx].metacarpal.swing.y, the_limit.fingers[finger_idx].mcp_swing_y);
231
+
out[acc_idx++] =
232
+
ModelToLM(in.finger[finger_idx].metacarpal.twist, the_limit.fingers[finger_idx].mcp_twist);
233
+
234
+
out[acc_idx++] =
235
+
ModelToLM(in.finger[finger_idx].proximal_swing.x, the_limit.fingers[finger_idx].pxm_swing_x);
236
+
out[acc_idx++] =
237
+
ModelToLM(in.finger[finger_idx].proximal_swing.y, the_limit.fingers[finger_idx].pxm_swing_y);
238
+
239
+
out[acc_idx++] = ModelToLM(in.finger[finger_idx].rots[0], the_limit.fingers[finger_idx].curls[0]);
240
+
out[acc_idx++] = ModelToLM(in.finger[finger_idx].rots[1], the_limit.fingers[finger_idx].curls[1]);
241
+
}
242
+
#endif
243
+
244
+
#ifdef USE_HAND_SIZE
245
+
if (use_hand_size) {
246
+
out[acc_idx++] = ModelToLM(in.hand_size, the_limit.hand_size);
247
+
}
248
+
#endif
249
+
}
250
+
251
+
template <typename T>
252
+
void
253
+
OptimizerHandInit(OptimizerHand<T> &opt, Quat<T> &pre_rotation)
254
+
{
255
+
opt.hand_size = (T)(0.095);
256
+
257
+
opt.wrist_post_orientation_aax.x = (T)(0);
258
+
opt.wrist_post_orientation_aax.y = (T)(0);
259
+
opt.wrist_post_orientation_aax.z = (T)(0);
260
+
261
+
// opt.store_wrist_pre_orientation_quat = pre_rotation;
262
+
263
+
opt.wrist_pre_orientation_quat.w = (T)pre_rotation.w;
264
+
opt.wrist_pre_orientation_quat.x = (T)pre_rotation.x;
265
+
opt.wrist_pre_orientation_quat.y = (T)pre_rotation.y;
266
+
opt.wrist_pre_orientation_quat.z = (T)pre_rotation.z;
267
+
268
+
opt.wrist_location.x = (T)(0);
269
+
opt.wrist_location.y = (T)(0);
270
+
opt.wrist_location.z = (T)(-0.3);
271
+
272
+
273
+
for (int i = 0; i < 4; i++) {
274
+
//!@todo needed?
275
+
opt.finger[i].metacarpal.swing.x = T(0);
276
+
opt.finger[i].metacarpal.twist = T(0);
277
+
278
+
opt.finger[i].proximal_swing.x = rad<T>((T)(15));
279
+
opt.finger[i].rots[0] = rad<T>((T)(-5));
280
+
opt.finger[i].rots[1] = rad<T>((T)(-5));
281
+
}
282
+
283
+
opt.thumb.metacarpal.swing.x = (T)(0);
284
+
opt.thumb.metacarpal.swing.y = (T)(0);
285
+
opt.thumb.metacarpal.twist = (T)(0);
286
+
287
+
opt.thumb.rots[0] = rad<T>((T)(-5));
288
+
opt.thumb.rots[1] = rad<T>((T)(-59));
289
+
290
+
opt.finger[0].metacarpal.swing.y = (T)(-0.19);
291
+
opt.finger[1].metacarpal.swing.y = (T)(0);
292
+
opt.finger[2].metacarpal.swing.y = (T)(0.19);
293
+
opt.finger[3].metacarpal.swing.y = (T)(0.38);
294
+
295
+
opt.finger[0].proximal_swing.y = (T)(-0.01);
296
+
opt.finger[1].proximal_swing.y = (T)(0);
297
+
opt.finger[2].proximal_swing.y = (T)(0.01);
298
+
opt.finger[3].proximal_swing.y = (T)(0.02);
299
+
}
300
+
301
+
// Applies the post axis-angle rotation to the pre quat, then zeroes out the axis-angle.
302
+
template <typename T>
303
+
void
304
+
OptimizerHandSquashRotations(OptimizerHand<T> &opt, Quat<T> &out_orientation)
305
+
{
306
+
307
+
// Hmmmmm, is this at all the right thing to do? :
308
+
opt.wrist_pre_orientation_quat.w = (T)out_orientation.w;
309
+
opt.wrist_pre_orientation_quat.x = (T)out_orientation.x;
310
+
opt.wrist_pre_orientation_quat.y = (T)out_orientation.y;
311
+
opt.wrist_pre_orientation_quat.z = (T)out_orientation.z;
312
+
313
+
Quat<T> &pre_rotation = opt.wrist_pre_orientation_quat;
314
+
315
+
Quat<T> post_rotation;
316
+
317
+
AngleAxisToQuaternion(opt.wrist_post_orientation_aax, post_rotation);
318
+
319
+
Quat<T> tmp_new_pre_rotation;
320
+
321
+
QuaternionProduct(pre_rotation, post_rotation, tmp_new_pre_rotation);
322
+
323
+
out_orientation.w = tmp_new_pre_rotation.w;
324
+
out_orientation.x = tmp_new_pre_rotation.x;
325
+
out_orientation.y = tmp_new_pre_rotation.y;
326
+
out_orientation.z = tmp_new_pre_rotation.z;
327
+
328
+
opt.wrist_post_orientation_aax.x = T(0);
329
+
opt.wrist_post_orientation_aax.y = T(0);
330
+
opt.wrist_post_orientation_aax.z = T(0);
331
+
}
332
+
333
+
334
+
} // namespace xrt::tracking::hand::mercury::lm
+244
src/xrt/tracking/hand/mercury/kine_lm/lm_rotations.inl
+244
src/xrt/tracking/hand/mercury/kine_lm/lm_rotations.inl
···
1
+
// Copyright 2022, Google, Inc.
2
+
// Copyright 2022, Collabora, Ltd.
3
+
// SPDX-License-Identifier: BSD-3-Clause
4
+
/*!
5
+
* @file
6
+
* @brief Autodiff-safe rotations for Levenberg-Marquardt kinematic optimizer.
7
+
* Copied out of Ceres's `rotation.h` with some modifications.
8
+
* @author Kier Mierle <kier@google.com>
9
+
* @author Sameer Agarwal <sameeragarwal@google.com>
10
+
* @author Moses Turner <moses@collabora.com>
11
+
* @ingroup tracking
12
+
*/
13
+
14
+
// Redistribution and use in source and binary forms, with or without
15
+
// modification, are permitted provided that the following conditions are met:
16
+
//
17
+
// * Redistributions of source code must retain the above copyright notice,
18
+
// this list of conditions and the following disclaimer.
19
+
// * Redistributions in binary form must reproduce the above copyright notice,
20
+
// this list of conditions and the following disclaimer in the documentation
21
+
// and/or other materials provided with the distribution.
22
+
// * Neither the name of Google Inc. nor the names of its contributors may be
23
+
// used to endorse or promote products derived from this software without
24
+
// specific prior written permission.
25
+
//
26
+
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
27
+
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
28
+
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
29
+
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
30
+
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
31
+
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
32
+
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
33
+
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
34
+
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
35
+
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
36
+
// POSSIBILITY OF SUCH DAMAGE.
37
+
38
+
#pragma once
39
+
#include <algorithm>
40
+
#include <cmath>
41
+
#include <limits>
42
+
#include "assert.h"
43
+
#include "float.h"
44
+
#include "lm_defines.hpp"
45
+
46
+
47
+
#define likely(x) __builtin_expect((x), 1)
48
+
#define unlikely(x) __builtin_expect((x), 0)
49
+
50
+
namespace xrt::tracking::hand::mercury::lm {
51
+
52
+
// For debugging.
53
+
#if 0
54
+
#include <iostream>
55
+
#define assert_quat_length_1(q) \
56
+
{ \
57
+
const T scale = q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]; \
58
+
if (abs(scale - T(1.0)) > 0.001) { \
59
+
std::cout << "Length bad! " << scale << std::endl; \
60
+
assert(false); \
61
+
}; \
62
+
}
63
+
#else
64
+
#define assert_quat_length_1(q)
65
+
#endif
66
+
67
+
68
+
template <typename T>
69
+
inline void
70
+
QuaternionProduct(const Quat<T> &z, const Quat<T> &w, Quat<T> &zw)
71
+
{
72
+
// Inplace product is not supported
73
+
assert(&z != &zw);
74
+
assert(&w != &zw);
75
+
76
+
assert_quat_length_1(z);
77
+
assert_quat_length_1(w);
78
+
79
+
80
+
// clang-format off
81
+
zw.w = z.w * w.w - z.x * w.x - z.y * w.y - z.z * w.z;
82
+
zw.x = z.w * w.x + z.x * w.w + z.y * w.z - z.z * w.y;
83
+
zw.y = z.w * w.y - z.x * w.z + z.y * w.w + z.z * w.x;
84
+
zw.z = z.w * w.z + z.x * w.y - z.y * w.x + z.z * w.w;
85
+
// clang-format on
86
+
}
87
+
88
+
89
+
template <typename T>
90
+
inline void
91
+
UnitQuaternionRotatePoint(const Quat<T> &q, const Vec3<T> &pt, Vec3<T> &result)
92
+
{
93
+
// clang-format off
94
+
T uv0 = q.y * pt.z - q.z * pt.y;
95
+
T uv1 = q.z * pt.x - q.x * pt.z;
96
+
T uv2 = q.x * pt.y - q.y * pt.x;
97
+
uv0 += uv0;
98
+
uv1 += uv1;
99
+
uv2 += uv2;
100
+
result.x = pt.x + q.w * uv0;
101
+
result.y = pt.y + q.w * uv1;
102
+
result.z = pt.z + q.w * uv2;
103
+
result.x += q.y * uv2 - q.z * uv1;
104
+
result.y += q.z * uv0 - q.x * uv2;
105
+
result.z += q.x * uv1 - q.y * uv0;
106
+
// clang-format on
107
+
}
108
+
109
+
template <typename T>
110
+
inline void
111
+
UnitQuaternionRotateAndScalePoint(const Quat<T> &q, const Vec3<T> &pt, const T scale, Vec3<T> &result)
112
+
{
113
+
T uv0 = q.y * pt.z - q.z * pt.y;
114
+
T uv1 = q.z * pt.x - q.x * pt.z;
115
+
T uv2 = q.x * pt.y - q.y * pt.x;
116
+
uv0 += uv0;
117
+
uv1 += uv1;
118
+
uv2 += uv2;
119
+
result.x = pt.x + q.w * uv0;
120
+
result.y = pt.y + q.w * uv1;
121
+
result.z = pt.z + q.w * uv2;
122
+
result.x += q.y * uv2 - q.z * uv1;
123
+
result.y += q.z * uv0 - q.x * uv2;
124
+
result.z += q.x * uv1 - q.y * uv0;
125
+
126
+
result.x *= scale;
127
+
result.y *= scale;
128
+
result.z *= scale;
129
+
}
130
+
131
+
132
+
template <typename T>
133
+
inline void
134
+
AngleAxisToQuaternion(const Vec3<T> angle_axis, Quat<T> &result)
135
+
{
136
+
const T &a0 = angle_axis.x;
137
+
const T &a1 = angle_axis.y;
138
+
const T &a2 = angle_axis.z;
139
+
const T theta_squared = a0 * a0 + a1 * a1 + a2 * a2;
140
+
141
+
// For points not at the origin, the full conversion is numerically stable.
142
+
if (likely(theta_squared > T(0.0))) {
143
+
const T theta = sqrt(theta_squared);
144
+
const T half_theta = theta * T(0.5);
145
+
const T k = sin(half_theta) / theta;
146
+
result.w = cos(half_theta);
147
+
result.x = a0 * k;
148
+
result.y = a1 * k;
149
+
result.z = a2 * k;
150
+
} else {
151
+
// At the origin, sqrt() will produce NaN in the derivative since
152
+
// the argument is zero. By approximating with a Taylor series,
153
+
// and truncating at one term, the value and first derivatives will be
154
+
// computed correctly when Jets are used.
155
+
const T k(0.5);
156
+
result.w = T(1.0);
157
+
result.x = a0 * k;
158
+
result.y = a1 * k;
159
+
result.z = a2 * k;
160
+
}
161
+
}
162
+
163
+
164
+
165
+
template <typename T>
166
+
inline void
167
+
CurlToQuaternion(const T &curl, Quat<T> &result)
168
+
{
169
+
const T theta_squared = curl * curl;
170
+
171
+
// For points not at the origin, the full conversion is numerically stable.
172
+
if (likely(theta_squared > T(0.0))) {
173
+
const T theta = curl;
174
+
const T half_theta = curl * T(0.5);
175
+
const T k = sin(half_theta) / theta;
176
+
result.w = cos(half_theta);
177
+
result.x = curl * k;
178
+
result.y = T(0.0);
179
+
result.z = T(0.0);
180
+
} else {
181
+
// At the origin, dividing by 0 is probably bad. By approximating with a Taylor series,
182
+
// and truncating at one term, the value and first derivatives will be
183
+
// computed correctly when Jets are used.
184
+
const T k(0.5);
185
+
result.w = T(1.0);
186
+
result.x = curl * k;
187
+
result.y = T(0.0);
188
+
result.z = T(0.0);
189
+
}
190
+
}
191
+
192
+
template <typename T>
193
+
inline void
194
+
SwingToQuaternion(const Vec2<T> swing, Quat<T> &result)
195
+
{
196
+
197
+
const T &a0 = swing.x;
198
+
const T &a1 = swing.y;
199
+
const T theta_squared = a0 * a0 + a1 * a1;
200
+
201
+
// For points not at the origin, the full conversion is numerically stable.
202
+
if (likely(theta_squared > T(0.0))) {
203
+
const T theta = sqrt(theta_squared);
204
+
const T half_theta = theta * T(0.5);
205
+
const T k = sin(half_theta) / theta;
206
+
result.w = cos(half_theta);
207
+
result.x = a0 * k;
208
+
result.y = a1 * k;
209
+
result.z = T(0);
210
+
} else {
211
+
// At the origin, sqrt() will produce NaN in the derivative since
212
+
// the argument is zero. By approximating with a Taylor series,
213
+
// and truncating at one term, the value and first derivatives will be
214
+
// computed correctly when Jets are used.
215
+
const T k(0.5);
216
+
result.w = T(1.0);
217
+
result.x = a0 * k;
218
+
result.y = a1 * k;
219
+
result.z = T(0);
220
+
}
221
+
}
222
+
223
+
template <typename T>
224
+
inline void
225
+
SwingTwistToQuaternion(const Vec2<T> swing, const T twist, Quat<T> &result)
226
+
{
227
+
//!@todo
228
+
// Rather than doing compound operations, we should derive it and collapse them.
229
+
Quat<T> swing_quat;
230
+
Quat<T> twist_quat;
231
+
232
+
Vec3<T> aax_twist;
233
+
234
+
aax_twist.x = (T)(0);
235
+
aax_twist.y = (T)(0);
236
+
aax_twist.z = twist;
237
+
238
+
SwingToQuaternion(swing, swing_quat);
239
+
240
+
AngleAxisToQuaternion(aax_twist, twist_quat);
241
+
242
+
QuaternionProduct(swing_quat, twist_quat, result);
243
+
}
244
+
} // namespace xrt::tracking::hand::mercury::lm