The open source OpenXR runtime
h/mercury: Add wrist pose initialization guesser
+237
-37
+4
-1
src/xrt/tracking/hand/mercury/kine_lm/CMakeLists.txt
+4
-1
src/xrt/tracking/hand/mercury/kine_lm/CMakeLists.txt
···
1
1
# Copyright 2022, Collabora, Ltd.
2
2
# SPDX-License-Identifier: BSL-1.0
3
3
4
-
add_library(t_ht_mercury_kine_lm STATIC lm_interface.hpp lm_main.cpp)
4
+
add_library(
5
+
t_ht_mercury_kine_lm STATIC lm_interface.hpp lm_main.cpp lm_hand_init_guesser.hpp
6
+
lm_hand_init_guesser.cpp
7
+
)
5
8
6
9
target_link_libraries(
7
10
t_ht_mercury_kine_lm
+181
src/xrt/tracking/hand/mercury/kine_lm/lm_hand_init_guesser.cpp
+181
src/xrt/tracking/hand/mercury/kine_lm/lm_hand_init_guesser.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
+
* @ingroup tracking
8
+
*/
9
+
10
+
#include "math/m_api.h"
11
+
#include "math/m_vec3.h"
12
+
#include "math/m_eigen_interop.hpp"
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
+
#include "../hg_numerics_checker.hpp"
30
+
#include "../hg_stereographic_unprojection.hpp"
31
+
32
+
namespace xrt::tracking::hand::mercury::lm {
33
+
using namespace xrt::auxiliary::math;
34
+
35
+
// Generated from mercury_train's hand_depth_guess.ipynb.
36
+
float
37
+
sympy_guess_distance(float angle, float wrist_extra_distance_meters, float hand_size)
38
+
{
39
+
float d1 = (1.0 / 2.0) *
40
+
(-wrist_extra_distance_meters * (angle - 1) +
41
+
sqrt((angle - 1) * (angle * pow(wrist_extra_distance_meters, 2) - 2 * pow(hand_size, 2) +
42
+
pow(wrist_extra_distance_meters, 2)))) /
43
+
(angle - 1);
44
+
float d2 = -(wrist_extra_distance_meters * (angle - 1) +
45
+
sqrt((angle - 1) * (angle * pow(wrist_extra_distance_meters, 2) - 2 * pow(hand_size, 2) +
46
+
pow(wrist_extra_distance_meters, 2)))) /
47
+
(2 * angle - 2);
48
+
49
+
if (d1 > d2) {
50
+
return d1;
51
+
}
52
+
return d2;
53
+
}
54
+
55
+
bool
56
+
hand_init_guess(one_frame_input &observation, const float hand_size, xrt_pose left_in_right, xrt_pose &out_wrist_guess)
57
+
{
58
+
int num_observation_views = 0;
59
+
60
+
61
+
// We're actually going "forwards" in our transformations for once!
62
+
// For the right camera, instead of moving an estimation *into* right-camera-space and doing math in
63
+
// camera-space, we move everything into *left-camera-space*.
64
+
xrt_pose right_in_left;
65
+
math_pose_invert(&left_in_right, &right_in_left);
66
+
67
+
xrt_vec3 wrist_global_sum = {};
68
+
xrt_vec3 midpxm_global_sum = {};
69
+
xrt_vec3 indpxm_global_sum = {};
70
+
xrt_vec3 litpxm_global_sum = {};
71
+
72
+
for (int view = 0; view < 2; view++) {
73
+
one_frame_one_view &v = observation.views[view];
74
+
if (!v.active) {
75
+
continue;
76
+
}
77
+
num_observation_views++;
78
+
79
+
Eigen::Quaternionf rotate = map_quat(v.look_dir);
80
+
Eigen::Vector3f directions_rel_camera[21];
81
+
82
+
for (int i = 0; i < 21; i++) {
83
+
float sg_x = v.keypoints_in_scaled_stereographic[i].pos_2d.x * v.stereographic_radius;
84
+
float sg_y = v.keypoints_in_scaled_stereographic[i].pos_2d.y * v.stereographic_radius;
85
+
directions_rel_camera[i] = stereographic_unprojection(sg_x, sg_y);
86
+
directions_rel_camera[i] = rotate * directions_rel_camera[i];
87
+
}
88
+
89
+
xrt_vec3 midpxm_dir;
90
+
xrt_vec3 wrist_dir;
91
+
map_vec3(midpxm_dir) = directions_rel_camera[Joint21::MIDL_PXM];
92
+
map_vec3(wrist_dir) = directions_rel_camera[Joint21::WRIST];
93
+
94
+
float angle = cos(m_vec3_angle(midpxm_dir, wrist_dir));
95
+
float wrist_extra_distance_meters =
96
+
v.keypoints_in_scaled_stereographic[0].depth_relative_to_midpxm * hand_size;
97
+
98
+
float distance = sympy_guess_distance(angle, wrist_extra_distance_meters, hand_size);
99
+
100
+
if (distance != distance) {
101
+
// Nan check.
102
+
// This happens if the angle between midpxm_dir and wrist_dir is 0, generally when
103
+
// refine_center_of_distribution fails hard enough. Generally not worth tracking hands when this
104
+
// happens.
105
+
return false;
106
+
}
107
+
108
+
// I wish I could make this an inline unit test. Useful for debugging so I'll leave it disabled.
109
+
#if 0
110
+
Eigen::Vector3f wrist_rel_camera = directions_rel_camera[0] * (distance+wrist_extra_distance_meters);
111
+
Eigen::Vector3f midpxm_rel_camera = directions_rel_camera[9] * distance;
112
+
113
+
float norm = (wrist_rel_camera-midpxm_rel_camera).norm();
114
+
U_LOG_E("Recovered: %f", norm);
115
+
#endif
116
+
117
+
xrt_vec3 wrist_rel_camera = {};
118
+
xrt_vec3 midpxm_rel_camera = {};
119
+
xrt_vec3 indpxm_rel_camera = {};
120
+
xrt_vec3 litpxm_rel_camera = {};
121
+
122
+
map_vec3(wrist_rel_camera) =
123
+
directions_rel_camera[Joint21::WRIST] * (distance + wrist_extra_distance_meters);
124
+
map_vec3(midpxm_rel_camera) = directions_rel_camera[Joint21::MIDL_PXM] * (distance);
125
+
map_vec3(indpxm_rel_camera) =
126
+
directions_rel_camera[Joint21::INDX_PXM] *
127
+
(distance +
128
+
(v.keypoints_in_scaled_stereographic[Joint21::INDX_PXM].depth_relative_to_midpxm * hand_size));
129
+
map_vec3(litpxm_rel_camera) =
130
+
directions_rel_camera[Joint21::LITL_PXM] *
131
+
(distance +
132
+
(v.keypoints_in_scaled_stereographic[Joint21::LITL_PXM].depth_relative_to_midpxm * hand_size));
133
+
134
+
if (view == 1) {
135
+
math_pose_transform_point(&right_in_left, &wrist_rel_camera, &wrist_rel_camera);
136
+
math_pose_transform_point(&right_in_left, &midpxm_rel_camera, &midpxm_rel_camera);
137
+
math_pose_transform_point(&right_in_left, &indpxm_rel_camera, &indpxm_rel_camera);
138
+
math_pose_transform_point(&right_in_left, &litpxm_rel_camera, &litpxm_rel_camera);
139
+
}
140
+
141
+
wrist_global_sum += wrist_rel_camera;
142
+
midpxm_global_sum += midpxm_rel_camera;
143
+
indpxm_global_sum += indpxm_rel_camera;
144
+
litpxm_global_sum += litpxm_rel_camera;
145
+
}
146
+
147
+
wrist_global_sum = m_vec3_mul_scalar(wrist_global_sum, 1.0f / (float)num_observation_views);
148
+
midpxm_global_sum = m_vec3_mul_scalar(midpxm_global_sum, 1.0f / (float)num_observation_views);
149
+
indpxm_global_sum = m_vec3_mul_scalar(indpxm_global_sum, 1.0f / (float)num_observation_views);
150
+
litpxm_global_sum = m_vec3_mul_scalar(wrist_global_sum, 1.0f / (float)num_observation_views);
151
+
152
+
out_wrist_guess.position = wrist_global_sum;
153
+
154
+
#if 0
155
+
// Original
156
+
xrt_vec3 plus_z = m_vec3_normalize(wrist_global_sum - midpxm_global_sum);
157
+
xrt_vec3 plus_x = m_vec3_normalize(indpxm_global_sum - litpxm_global_sum);
158
+
159
+
#elif 0
160
+
// Negated
161
+
xrt_vec3 plus_z = m_vec3_normalize(midpxm_global_sum - wrist_global_sum);
162
+
// Not negated
163
+
xrt_vec3 plus_x = m_vec3_normalize(indpxm_global_sum - litpxm_global_sum);
164
+
#elif 0
165
+
// Both negated
166
+
xrt_vec3 plus_z = m_vec3_normalize(midpxm_global_sum - wrist_global_sum);
167
+
xrt_vec3 plus_x = m_vec3_normalize(litpxm_global_sum - indpxm_global_sum);
168
+
169
+
#else
170
+
// Not negated
171
+
xrt_vec3 plus_z = m_vec3_normalize(wrist_global_sum - midpxm_global_sum);
172
+
// Negated
173
+
xrt_vec3 plus_x = m_vec3_normalize(litpxm_global_sum - indpxm_global_sum);
174
+
#endif
175
+
176
+
plus_x = m_vec3_orthonormalize(plus_z, plus_x);
177
+
math_quat_from_plus_x_z(&plus_x, &plus_z, &out_wrist_guess.orientation);
178
+
179
+
return true;
180
+
}
181
+
} // namespace xrt::tracking::hand::mercury::lm
+22
src/xrt/tracking/hand/mercury/kine_lm/lm_hand_init_guesser.hpp
+22
src/xrt/tracking/hand/mercury/kine_lm/lm_hand_init_guesser.hpp
···
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
+
* @ingroup tracking
8
+
*/
9
+
10
+
#pragma once
11
+
#include "math/m_api.h"
12
+
#include "math/m_vec3.h"
13
+
14
+
#include "lm_defines.hpp"
15
+
16
+
17
+
namespace xrt::tracking::hand::mercury::lm {
18
+
19
+
bool
20
+
hand_init_guess(one_frame_input &observation, const float hand_size, xrt_pose left_in_right, xrt_pose &out_wrist_guess);
21
+
22
+
} // namespace xrt::tracking::hand::mercury::lm
+30
-36
src/xrt/tracking/hand/mercury/kine_lm/lm_main.cpp
+30
-36
src/xrt/tracking/hand/mercury/kine_lm/lm_main.cpp
···
26
26
#include "lm_defines.hpp"
27
27
28
28
#include "../hg_numerics_checker.hpp"
29
+
#include "lm_hand_init_guesser.hpp"
29
30
30
31
/*
31
32
···
954
955
}
955
956
956
957
void
957
-
hand_was_untracked(KinematicHandLM &state)
958
-
{
959
-
state.first_frame = true;
960
-
#if 0
961
-
state.last_frame_pre_rotation.w = 1.0;
962
-
state.last_frame_pre_rotation.x = 0.0;
963
-
state.last_frame_pre_rotation.y = 0.0;
964
-
state.last_frame_pre_rotation.z = 0.0;
965
-
#else
966
-
// Rotated 90 degrees so that the palm is facing the user and the fingers are up.
967
-
// The _idea_ is that having the palm be "in" the camera's plane will reduce initial local minima due to flat
968
-
// things having multiple possible unprojections.
969
-
state.this_frame_pre_rotation.w = sqrt(2) / 2;
970
-
state.this_frame_pre_rotation.x = -sqrt(2) / 2;
971
-
state.this_frame_pre_rotation.y = 0.0;
972
-
state.this_frame_pre_rotation.z = 0.0;
973
-
#endif
974
-
975
-
state.this_frame_pre_position.x = 0.0f;
976
-
state.this_frame_pre_position.y = 0.0f;
977
-
state.this_frame_pre_position.z = -0.3f;
978
-
979
-
OptimizerHandInit(state.last_frame, state.this_frame_pre_rotation);
980
-
OptimizerHandPackIntoVector(state.last_frame, state.optimize_hand_size, state.TinyOptimizerInput.data());
981
-
}
982
-
983
-
void
984
958
optimizer_run(KinematicHandLM *hand,
985
959
one_frame_input &observation,
986
960
bool hand_was_untracked_last_frame,
···
998
972
KinematicHandLM &state = *hand;
999
973
state.smoothing_factor = smoothing_factor;
1000
974
975
+
xrt_pose blah = XRT_POSE_IDENTITY;
976
+
hand_init_guess(observation, target_hand_size, state.left_in_right, blah);
977
+
1001
978
if (hand_was_untracked_last_frame) {
1002
-
hand_was_untracked(state);
979
+
OptimizerHandInit(state.last_frame, state.this_frame_pre_rotation);
980
+
OptimizerHandPackIntoVector(state.last_frame, state.optimize_hand_size,
981
+
state.TinyOptimizerInput.data());
982
+
if (blah.position.z > 0.05) {
983
+
LM_WARN(state, "Initial position guess was too close to camera! Z axis was %f m",
984
+
blah.position.z);
985
+
state.this_frame_pre_position.x = 0.0f;
986
+
state.this_frame_pre_position.y = 0.0f;
987
+
state.this_frame_pre_position.z = -0.3f;
988
+
} else {
989
+
state.this_frame_pre_position.x = blah.position.x;
990
+
state.this_frame_pre_position.y = blah.position.y;
991
+
state.this_frame_pre_position.z = blah.position.z;
992
+
}
993
+
994
+
995
+
996
+
state.this_frame_pre_rotation.x = blah.orientation.x;
997
+
state.this_frame_pre_rotation.y = blah.orientation.y;
998
+
state.this_frame_pre_rotation.z = blah.orientation.z;
999
+
state.this_frame_pre_rotation.w = blah.orientation.w;
1003
1000
}
1004
1001
1005
1002
state.num_observation_views = 0;
···
1072
1069
1073
1070
1074
1071
1075
-
// Postfix - unpack,
1072
+
// Postfix - unpack our optimization result into state.last_frame.
1076
1073
OptimizerHandUnpackFromVector(state.TinyOptimizerInput.data(), state, state.last_frame);
1077
1074
1078
1075
1079
1076
1080
-
// Squash the orientations
1077
+
// Have the final pose from this frame be the next frame's initial pose
1081
1078
state.this_frame_pre_rotation = state.last_frame.wrist_final_orientation;
1082
1079
state.this_frame_pre_position = state.last_frame.wrist_final_location;
1083
-
// Repack - brings the curl values back into original domain. Look at ModelToLM/LMToModel, we're
1084
-
// using sin/asin.
1085
1080
1081
+
// Reset this frame's post-transform to identity
1086
1082
state.last_frame.wrist_post_location.x = 0.0f;
1087
1083
state.last_frame.wrist_post_location.y = 0.0f;
1088
1084
state.last_frame.wrist_post_location.z = 0.0f;
···
1091
1087
state.last_frame.wrist_post_orientation_aax.y = 0.0f;
1092
1088
state.last_frame.wrist_post_orientation_aax.z = 0.0f;
1093
1089
1094
-
1090
+
// Repack - brings the curl values back into original domain. Look at ModelToLM/LMToModel, we're
1091
+
// using sin/asin.
1095
1092
OptimizerHandPackIntoVector(state.last_frame, hand->optimize_hand_size, state.TinyOptimizerInput.data());
1096
1093
1097
1094
optimizer_finish(state, out_viz_hand, out_reprojection_error);
···
1121
1118
hand->left_in_right_orientation.x = left_in_right.orientation.x;
1122
1119
hand->left_in_right_orientation.y = left_in_right.orientation.y;
1123
1120
hand->left_in_right_orientation.z = left_in_right.orientation.z;
1124
-
1125
-
// Probably unnecessary.
1126
-
hand_was_untracked(*hand);
1127
1121
1128
1122
*out_kinematic_hand = hand;
1129
1123
}