The open source OpenXR runtime
h/mercury: Have a post-translation as well as post-rotation
For better numerical stability. Shouldn't significantly affect tracking besides slight jitter reduction and very slightly higher accuracy.
+96
-104
+10
-6
src/xrt/tracking/hand/mercury/kine_lm/lm_defines.hpp
+10
-6
src/xrt/tracking/hand/mercury/kine_lm/lm_defines.hpp
···
386
386
template <typename T> struct OptimizerHand
387
387
{
388
388
T hand_size;
389
-
Vec3<T> wrist_location = {};
390
-
// This is constant, a ceres::Rotation.h quat,, taken from last frame.
391
-
Quat<T> wrist_pre_orientation_quat = {};
392
-
// This is optimized - angle-axis rotation vector. Starts at 0, loss goes up the higher it goes because it
393
-
// indicates more of a rotation.
389
+
Vec3<T> wrist_post_location = {};
394
390
Vec3<T> wrist_post_orientation_aax = {};
391
+
392
+
Vec3<T> wrist_final_location = {};
393
+
Quat<T> wrist_final_orientation = {};
394
+
395
395
OptimizerThumb<T> thumb = {};
396
396
OptimizerFinger<T> finger[4] = {};
397
397
};
···
517
517
518
518
u_logging_level log_level = U_LOGGING_INFO;
519
519
520
-
Quat<HandScalar> last_frame_pre_rotation = {};
520
+
// Squashed final pose from last frame. We start from here.
521
+
// At some point this might turn into a pose-prediction instead, we'll see :)
522
+
Quat<HandScalar> this_frame_pre_rotation = {};
523
+
Vec3<HandScalar> this_frame_pre_position = {};
524
+
521
525
OptimizerHand<HandScalar> last_frame = {};
522
526
523
527
// The pose that will take you from the right camera's space to the left camera's space.
+55
-53
src/xrt/tracking/hand/mercury/kine_lm/lm_main.cpp
+55
-53
src/xrt/tracking/hand/mercury/kine_lm/lm_main.cpp
···
168
168
169
169
template <typename T>
170
170
void
171
-
eval_hand_with_orientation(const OptimizerHand<T> &opt,
171
+
eval_hand_with_orientation(const KinematicHandLM &state,
172
+
const OptimizerHand<T> &opt,
172
173
const bool is_right,
173
174
Translations55<T> &translations_absolute,
174
175
Orientations54<T> &orientations_absolute)
···
184
185
185
186
eval_hand_set_rel_translations(opt, rel_translations);
186
187
187
-
Quat<T> orientation_root = {};
188
-
189
-
Quat<T> post_orientation_quat = {};
190
-
191
-
AngleAxisToQuaternion(opt.wrist_post_orientation_aax, post_orientation_quat);
192
-
193
-
QuaternionProduct(opt.wrist_pre_orientation_quat, post_orientation_quat, orientation_root);
194
188
195
189
// Get each joint's tracking-relative orientation by rotating its parent-relative orientation by the
196
190
// tracking-relative orientation of its parent.
197
191
for (size_t finger = 0; finger < kNumFingers; finger++) {
198
-
Quat<T> *last_orientation = &orientation_root;
192
+
const Quat<T> *last_orientation = &opt.wrist_final_orientation;
199
193
for (size_t bone = 0; bone < kNumOrientationsInFinger; bone++) {
200
194
Quat<T> &out_orientation = orientations_absolute.q[finger][bone];
201
195
Quat<T> &rel_orientation = rel_orientations.q[finger][bone];
···
205
199
}
206
200
}
207
201
202
+
208
203
// Get each joint's tracking-relative position by rotating its parent-relative translation by the
209
204
// tracking-relative orientation of its parent, then adding that to its parent's tracking-relative position.
210
205
for (size_t finger = 0; finger < kNumFingers; finger++) {
211
-
const Vec3<T> *last_translation = &opt.wrist_location;
212
-
const Quat<T> *last_orientation = &orientation_root;
206
+
const Vec3<T> *last_translation = &opt.wrist_final_location;
207
+
const Quat<T> *last_orientation = &opt.wrist_final_orientation;
213
208
for (size_t bone = 0; bone < kNumJointsInFinger; bone++) {
214
209
Vec3<T> &out_translation = translations_absolute.t[finger][bone];
215
210
Vec3<T> &rel_translation = rel_translations.t[finger][bone];
···
328
323
return;
329
324
}
330
325
331
-
helper.AddValue((last_hand.wrist_location.x - hand.wrist_location.x) * stab.stabilityRootPosition);
332
-
helper.AddValue((last_hand.wrist_location.y - hand.wrist_location.y) * stab.stabilityRootPosition);
333
-
helper.AddValue((last_hand.wrist_location.z - hand.wrist_location.z) * stab.stabilityRootPosition);
326
+
helper.AddValue((hand.wrist_post_location.x) * stab.stabilityRootPosition);
327
+
helper.AddValue((hand.wrist_post_location.y) * stab.stabilityRootPosition);
328
+
helper.AddValue((hand.wrist_post_location.z) * stab.stabilityRootPosition);
334
329
335
330
336
331
helper.AddValue((hand.wrist_post_orientation_aax.x) * (T)(stab.stabilityHandOrientationXY));
···
493
488
494
489
int joint_acc_idx = 0;
495
490
496
-
calc_joint_rel_camera(hand.wrist_location, move_direction, move_orientation, after_orientation,
491
+
Vec3<T> root = state.this_frame_pre_position;
492
+
root.x += hand.wrist_post_location.x;
493
+
root.y += hand.wrist_post_location.y;
494
+
root.z += hand.wrist_post_location.z;
495
+
496
+
497
+
calc_joint_rel_camera(root, move_direction, move_orientation, after_orientation,
497
498
out_model_joints_rel_camera[joint_acc_idx++]);
498
499
499
500
for (int finger_idx = 0; finger_idx < 5; finger_idx++) {
···
664
665
struct KinematicHandLM &state = this->parent;
665
666
OptimizerHand<T> hand = {};
666
667
// ??? should I do the below? probably.
667
-
Quat<T> tmp = this->parent.last_frame_pre_rotation;
668
+
Quat<T> tmp = this->parent.this_frame_pre_rotation;
668
669
OptimizerHandInit<T>(hand, tmp);
669
-
OptimizerHandUnpackFromVector(x, state.optimize_hand_size, T(state.target_hand_size), hand);
670
+
OptimizerHandUnpackFromVector(x, state, hand);
670
671
671
672
XRT_MAYBE_UNUSED size_t residual_size =
672
673
calc_residual_size(state.use_stability, optimize_hand_size, state.num_observation_views);
···
685
686
686
687
Translations55<T> translations_absolute = {};
687
688
Orientations54<T> orientations_absolute = {};
688
-
eval_hand_with_orientation(hand, state.is_right, translations_absolute, orientations_absolute);
689
+
eval_hand_with_orientation(state, hand, state.is_right, translations_absolute, orientations_absolute);
689
690
690
691
691
692
CostFunctor_PositionsPart<T>(hand, translations_absolute, state, helper);
···
785
786
{
786
787
XRT_TRACE_MARKER();
787
788
788
-
eval_hand_with_orientation(opt, state.is_right, translations_absolute, orientations_absolute);
789
-
790
-
Quat<HandScalar> post_wrist_orientation = {};
791
-
792
-
AngleAxisToQuaternion(opt.wrist_post_orientation_aax, post_wrist_orientation);
793
-
794
-
Quat<HandScalar> pre_wrist_orientation = state.last_frame_pre_rotation;
795
-
796
-
797
-
Quat<HandScalar> final_wrist_orientation = {};
798
-
799
-
QuaternionProduct(pre_wrist_orientation, post_wrist_orientation, final_wrist_orientation);
789
+
eval_hand_with_orientation(state, opt, state.is_right, translations_absolute, orientations_absolute);
800
790
801
791
int joint_acc_idx = 0;
802
792
···
812
802
zldtt(palm_position, palm_orientation, state.is_right,
813
803
out_viz_hand.values.hand_joint_set_default[joint_acc_idx++].relation);
814
804
805
+
815
806
// Wrist.
816
-
zldtt(opt.wrist_location, final_wrist_orientation, state.is_right,
807
+
zldtt(opt.wrist_final_location, opt.wrist_final_orientation, state.is_right,
817
808
out_viz_hand.values.hand_joint_set_default[joint_acc_idx++].relation);
818
809
819
810
for (int finger = 0; finger < 5; finger++) {
···
920
911
921
912
Translations55<HandScalar> translations_absolute;
922
913
Orientations54<HandScalar> orientations_absolute;
923
-
eval_hand_with_orientation<HandScalar>(final_hand, state.is_right, translations_absolute,
914
+
eval_hand_with_orientation<HandScalar>(state, final_hand, state.is_right, translations_absolute,
924
915
orientations_absolute);
925
916
926
917
eval_to_viz_hand(state, final_hand, translations_absolute, orientations_absolute, out_viz_hand);
···
945
936
out_reprojection_error = sum;
946
937
}
947
938
948
-
949
939
void
950
-
hand_was_untracked(KinematicHandLM *hand)
940
+
hand_was_untracked(KinematicHandLM &state)
951
941
{
952
-
hand->first_frame = true;
942
+
state.first_frame = true;
953
943
#if 0
954
-
hand->last_frame_pre_rotation.w = 1.0;
955
-
hand->last_frame_pre_rotation.x = 0.0;
956
-
hand->last_frame_pre_rotation.y = 0.0;
957
-
hand->last_frame_pre_rotation.z = 0.0;
944
+
state.last_frame_pre_rotation.w = 1.0;
945
+
state.last_frame_pre_rotation.x = 0.0;
946
+
state.last_frame_pre_rotation.y = 0.0;
947
+
state.last_frame_pre_rotation.z = 0.0;
958
948
#else
959
949
// Rotated 90 degrees so that the palm is facing the user and the fingers are up.
960
950
// The _idea_ is that having the palm be "in" the camera's plane will reduce initial local minima due to flat
961
951
// things having multiple possible unprojections.
962
-
hand->last_frame_pre_rotation.w = sqrt(2) / 2;
963
-
hand->last_frame_pre_rotation.x = -sqrt(2) / 2;
964
-
hand->last_frame_pre_rotation.y = 0.0;
965
-
hand->last_frame_pre_rotation.z = 0.0;
952
+
state.this_frame_pre_rotation.w = sqrt(2) / 2;
953
+
state.this_frame_pre_rotation.x = -sqrt(2) / 2;
954
+
state.this_frame_pre_rotation.y = 0.0;
955
+
state.this_frame_pre_rotation.z = 0.0;
966
956
#endif
967
957
968
-
OptimizerHandInit(hand->last_frame, hand->last_frame_pre_rotation);
969
-
OptimizerHandPackIntoVector(hand->last_frame, hand->optimize_hand_size, hand->TinyOptimizerInput.data());
958
+
state.this_frame_pre_position.x = 0.0f;
959
+
state.this_frame_pre_position.y = 0.0f;
960
+
state.this_frame_pre_position.z = -0.3f;
961
+
962
+
OptimizerHandInit(state.last_frame, state.this_frame_pre_rotation);
963
+
OptimizerHandPackIntoVector(state.last_frame, state.optimize_hand_size, state.TinyOptimizerInput.data());
970
964
}
971
965
972
966
void
···
988
982
state.smoothing_factor = smoothing_factor;
989
983
990
984
if (hand_was_untracked_last_frame) {
991
-
hand_was_untracked(hand);
985
+
hand_was_untracked(state);
992
986
}
993
987
994
988
state.num_observation_views = 0;
···
1017
1011
#if 0
1018
1012
1019
1013
OptimizerHand<HandScalar> blah = {};
1020
-
OptimizerHandUnpackFromVector(state.TinyOptimizerInput.data(), state.optimize_hand_size, state.target_hand_size,
1021
-
blah);
1014
+
OptimizerHandUnpackFromVector(state.TinyOptimizerInput.data(), state, blah);
1022
1015
1023
1016
for (int finger_idx = 0; finger_idx < 4; finger_idx++) {
1024
1017
// int finger_idx = 0;
···
1063
1056
1064
1057
1065
1058
// Postfix - unpack,
1066
-
OptimizerHandUnpackFromVector(state.TinyOptimizerInput.data(), state.optimize_hand_size, state.target_hand_size,
1067
-
state.last_frame);
1059
+
OptimizerHandUnpackFromVector(state.TinyOptimizerInput.data(), state, state.last_frame);
1068
1060
1069
1061
1070
1062
1071
1063
// Squash the orientations
1072
-
OptimizerHandSquashRotations(state.last_frame, state.last_frame_pre_rotation);
1073
-
1064
+
state.this_frame_pre_rotation = state.last_frame.wrist_final_orientation;
1065
+
state.this_frame_pre_position = state.last_frame.wrist_final_location;
1074
1066
// Repack - brings the curl values back into original domain. Look at ModelToLM/LMToModel, we're
1075
1067
// using sin/asin.
1068
+
1069
+
state.last_frame.wrist_post_location.x = 0.0f;
1070
+
state.last_frame.wrist_post_location.y = 0.0f;
1071
+
state.last_frame.wrist_post_location.z = 0.0f;
1072
+
1073
+
state.last_frame.wrist_post_orientation_aax.x = 0.0f;
1074
+
state.last_frame.wrist_post_orientation_aax.y = 0.0f;
1075
+
state.last_frame.wrist_post_orientation_aax.z = 0.0f;
1076
+
1077
+
1076
1078
OptimizerHandPackIntoVector(state.last_frame, hand->optimize_hand_size, state.TinyOptimizerInput.data());
1077
1079
1078
1080
optimizer_finish(state, out_viz_hand, out_reprojection_error);
···
1104
1106
hand->left_in_right_orientation.z = left_in_right.orientation.z;
1105
1107
1106
1108
// Probably unnecessary.
1107
-
hand_was_untracked(hand);
1109
+
hand_was_untracked(*hand);
1108
1110
1109
1111
*out_kinematic_hand = hand;
1110
1112
}
+31
-45
src/xrt/tracking/hand/mercury/kine_lm/lm_optimizer_params_packer.inl
+31
-45
src/xrt/tracking/hand/mercury/kine_lm/lm_optimizer_params_packer.inl
···
15
15
16
16
#include "lm_interface.hpp"
17
17
#include "lm_defines.hpp"
18
+
#include "lm_rotations.inl"
18
19
19
20
namespace xrt::tracking::hand::mercury::lm {
20
21
···
37
38
// Input vector,
38
39
template <typename T>
39
40
void
40
-
OptimizerHandUnpackFromVector(const T *in, bool use_hand_size, T hand_size, OptimizerHand<T> &out)
41
+
OptimizerHandUnpackFromVector(const T *in, const KinematicHandLM &state, OptimizerHand<T> &out)
41
42
{
43
+
44
+
const Quat<T> pre_wrist_orientation(state.this_frame_pre_rotation);
45
+
const Vec3<T> pre_wrist_position(state.this_frame_pre_position);
42
46
43
47
size_t acc_idx = 0;
44
48
#ifdef USE_HAND_TRANSLATION
45
-
out.wrist_location.x = in[acc_idx++];
46
-
out.wrist_location.y = in[acc_idx++];
47
-
out.wrist_location.z = in[acc_idx++];
49
+
out.wrist_post_location.x = in[acc_idx++];
50
+
out.wrist_post_location.y = in[acc_idx++];
51
+
out.wrist_post_location.z = in[acc_idx++];
52
+
53
+
out.wrist_final_location.x = out.wrist_post_location.x + T(pre_wrist_position.x);
54
+
out.wrist_final_location.y = out.wrist_post_location.y + T(pre_wrist_position.y);
55
+
out.wrist_final_location.z = out.wrist_post_location.z + T(pre_wrist_position.z);
56
+
48
57
#endif
58
+
49
59
#ifdef USE_HAND_ORIENTATION
50
60
out.wrist_post_orientation_aax.x = in[acc_idx++];
51
61
out.wrist_post_orientation_aax.y = in[acc_idx++];
52
62
out.wrist_post_orientation_aax.z = in[acc_idx++];
63
+
64
+
Quat<T> post_wrist_orientation = {};
65
+
66
+
AngleAxisToQuaternion<T>(out.wrist_post_orientation_aax, post_wrist_orientation);
67
+
68
+
Quat<T> pre_wrist_orientation_t(pre_wrist_orientation);
69
+
70
+
QuaternionProduct<T>(pre_wrist_orientation_t, post_wrist_orientation, out.wrist_final_orientation);
53
71
#endif
54
72
55
73
#ifdef USE_EVERYTHING_ELSE
···
74
92
#endif
75
93
76
94
#ifdef USE_HAND_SIZE
77
-
if (use_hand_size) {
95
+
if (state.optimize_hand_size) {
78
96
out.hand_size = LMToModel(in[acc_idx++], the_limit.hand_size);
79
97
} else {
80
-
out.hand_size = hand_size;
98
+
out.hand_size = T(state.target_hand_size);
81
99
}
82
100
#endif
83
101
}
···
89
107
size_t acc_idx = 0;
90
108
91
109
#ifdef USE_HAND_TRANSLATION
92
-
out[acc_idx++] = in.wrist_location.x;
93
-
out[acc_idx++] = in.wrist_location.y;
94
-
out[acc_idx++] = in.wrist_location.z;
110
+
out[acc_idx++] = in.wrist_post_location.x;
111
+
out[acc_idx++] = in.wrist_post_location.y;
112
+
out[acc_idx++] = in.wrist_post_location.z;
95
113
#endif
96
114
#ifdef USE_HAND_ORIENTATION
97
115
out[acc_idx++] = in.wrist_post_orientation_aax.x;
···
136
154
opt.wrist_post_orientation_aax.z = (T)(0);
137
155
138
156
139
-
opt.wrist_pre_orientation_quat = pre_rotation;
157
+
// opt.wrist_pre_orientation_quat = pre_rotation;
140
158
141
-
opt.wrist_location.x = (T)(0);
142
-
opt.wrist_location.y = (T)(0);
143
-
opt.wrist_location.z = (T)(-0.3);
159
+
opt.wrist_post_location.x = (T)(0);
160
+
opt.wrist_post_location.y = (T)(0);
161
+
opt.wrist_post_location.z = (T)(0);
144
162
145
163
146
164
for (int i = 0; i < 4; i++) {
···
174
192
opt.finger[1].proximal_swing.y = (T)(0);
175
193
opt.finger[2].proximal_swing.y = (T)(0.01);
176
194
opt.finger[3].proximal_swing.y = (T)(0.02);
177
-
}
178
-
179
-
// Applies the post axis-angle rotation to the pre quat, then zeroes out the axis-angle.
180
-
template <typename T>
181
-
void
182
-
OptimizerHandSquashRotations(OptimizerHand<T> &opt, Quat<T> &out_orientation)
183
-
{
184
-
185
-
// Hmmmmm, is this at all the right thing to do? :
186
-
opt.wrist_pre_orientation_quat.w = (T)out_orientation.w;
187
-
opt.wrist_pre_orientation_quat.x = (T)out_orientation.x;
188
-
opt.wrist_pre_orientation_quat.y = (T)out_orientation.y;
189
-
opt.wrist_pre_orientation_quat.z = (T)out_orientation.z;
190
-
191
-
Quat<T> &pre_rotation = opt.wrist_pre_orientation_quat;
192
-
193
-
Quat<T> post_rotation = {};
194
-
195
-
AngleAxisToQuaternion(opt.wrist_post_orientation_aax, post_rotation);
196
-
197
-
Quat<T> tmp_new_pre_rotation = {};
198
-
199
-
QuaternionProduct(pre_rotation, post_rotation, tmp_new_pre_rotation);
200
-
201
-
out_orientation.w = tmp_new_pre_rotation.w;
202
-
out_orientation.x = tmp_new_pre_rotation.x;
203
-
out_orientation.y = tmp_new_pre_rotation.y;
204
-
out_orientation.z = tmp_new_pre_rotation.z;
205
-
206
-
opt.wrist_post_orientation_aax.x = T(0);
207
-
opt.wrist_post_orientation_aax.y = T(0);
208
-
opt.wrist_post_orientation_aax.z = T(0);
209
195
}
210
196
211
197