The open source OpenXR runtime
h/mercury: Move all defines to lm_defines.hpp
+206
-199
+206
src/xrt/tracking/hand/mercury/kine_lm/lm_defines.hpp
+206
src/xrt/tracking/hand/mercury/kine_lm/lm_defines.hpp
···
4
4
* @file
5
5
* @brief Defines for Levenberg-Marquardt kinematic optimizer
6
6
* @author Moses Turner <moses@collabora.com>
7
+
* @author Charlton Rodda <charlton.rodda@collabora.com>
7
8
* @ingroup tracking
8
9
*/
9
10
#pragma once
10
11
11
12
#include "math/m_mathinclude.h"
13
+
#include "math/m_eigen_interop.hpp"
14
+
#include "util/u_logging.h"
12
15
#include "../kine_common.hpp"
13
16
14
17
namespace xrt::tracking::hand::mercury::lm {
···
357
360
this->out_residual[out_residual_idx++] = value;
358
361
}
359
362
};
363
+
364
+
365
+
template <typename T> struct OptimizerMetacarpalBone
366
+
{
367
+
Vec2<T> swing = {};
368
+
T twist = {};
369
+
};
370
+
371
+
template <typename T> struct OptimizerFinger
372
+
{
373
+
OptimizerMetacarpalBone<T> metacarpal = {};
374
+
Vec2<T> proximal_swing = {};
375
+
// Not Vec2.
376
+
T rots[2] = {};
377
+
};
378
+
379
+
template <typename T> struct OptimizerThumb
380
+
{
381
+
OptimizerMetacarpalBone<T> metacarpal = {};
382
+
// Again not Vec2.
383
+
T rots[2] = {};
384
+
};
385
+
386
+
template <typename T> struct OptimizerHand
387
+
{
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.
394
+
Vec3<T> wrist_post_orientation_aax = {};
395
+
OptimizerThumb<T> thumb = {};
396
+
OptimizerFinger<T> finger[4] = {};
397
+
};
398
+
399
+
400
+
struct minmax
401
+
{
402
+
HandScalar min = 0;
403
+
HandScalar max = 0;
404
+
};
405
+
406
+
class FingerLimit
407
+
{
408
+
public:
409
+
minmax mcp_swing_x = {};
410
+
minmax mcp_swing_y = {};
411
+
minmax mcp_twist = {};
412
+
413
+
minmax pxm_swing_x = {};
414
+
minmax pxm_swing_y = {};
415
+
416
+
minmax curls[2] = {}; // int, dst
417
+
};
418
+
419
+
class HandLimit
420
+
{
421
+
public:
422
+
minmax hand_size = {};
423
+
424
+
minmax thumb_mcp_swing_x = {};
425
+
minmax thumb_mcp_swing_y = {};
426
+
minmax thumb_mcp_twist = {};
427
+
minmax thumb_curls[2] = {};
428
+
429
+
FingerLimit fingers[4] = {};
430
+
431
+
HandLimit()
432
+
{
433
+
hand_size = {MIN_HAND_SIZE, MAX_HAND_SIZE};
434
+
435
+
thumb_mcp_swing_x = {rad<HandScalar>(-60), rad<HandScalar>(60)};
436
+
thumb_mcp_swing_y = {rad<HandScalar>(-60), rad<HandScalar>(60)};
437
+
thumb_mcp_twist = {rad<HandScalar>(-35), rad<HandScalar>(35)};
438
+
439
+
for (int i = 0; i < 2; i++) {
440
+
thumb_curls[i] = {rad<HandScalar>(-90), rad<HandScalar>(40)};
441
+
}
442
+
443
+
444
+
HandScalar margin = 0.0001;
445
+
446
+
fingers[0].mcp_swing_y = {HandScalar(-0.19) - margin, HandScalar(-0.19) + margin};
447
+
fingers[1].mcp_swing_y = {HandScalar(0.00) - margin, HandScalar(0.00) + margin};
448
+
fingers[2].mcp_swing_y = {HandScalar(0.19) - margin, HandScalar(0.19) + margin};
449
+
fingers[3].mcp_swing_y = {HandScalar(0.38) - margin, HandScalar(0.38) + margin};
450
+
451
+
452
+
fingers[0].mcp_swing_x = {HandScalar(-0.02) - margin, HandScalar(-0.02) + margin};
453
+
fingers[1].mcp_swing_x = {HandScalar(0.00) - margin, HandScalar(0.00) + margin};
454
+
fingers[2].mcp_swing_x = {HandScalar(0.02) - margin, HandScalar(0.02) + margin};
455
+
fingers[3].mcp_swing_x = {HandScalar(0.04) - margin, HandScalar(0.04) + margin};
456
+
457
+
458
+
for (int finger_idx = 0; finger_idx < 4; finger_idx++) {
459
+
FingerLimit &finger = fingers[finger_idx];
460
+
461
+
// finger.mcp_swing_x = {rad<HandScalar>(-0.0001), rad<HandScalar>(0.0001)};
462
+
finger.mcp_twist = {rad<HandScalar>(-4), rad<HandScalar>(4)};
463
+
464
+
finger.pxm_swing_x = {rad<HandScalar>(-100), rad<HandScalar>(20)}; // ??? why is it reversed
465
+
finger.pxm_swing_y = {rad<HandScalar>(-20), rad<HandScalar>(20)};
466
+
467
+
for (int i = 0; i < 2; i++) {
468
+
finger.curls[i] = {rad<HandScalar>(-90), rad<HandScalar>(0)};
469
+
}
470
+
}
471
+
}
472
+
};
473
+
474
+
static const class HandLimit the_limit = {};
475
+
476
+
477
+
template <typename T> struct StereographicObservation
478
+
{
479
+
Vec2<T> obs[kNumNNJoints];
480
+
};
481
+
482
+
483
+
template <typename T> struct DepthObservation
484
+
{
485
+
T depth_value[kNumNNJoints];
486
+
};
487
+
488
+
template <typename T> struct ResidualTracker
489
+
{
490
+
T *out_residual = nullptr;
491
+
size_t out_residual_idx = {};
492
+
493
+
ResidualTracker(T *residual) : out_residual(residual) {}
494
+
495
+
void
496
+
AddValue(T const &value)
497
+
{
498
+
this->out_residual[out_residual_idx++] = value;
499
+
}
500
+
};
501
+
502
+
503
+
struct KinematicHandLM
504
+
{
505
+
bool first_frame = true;
506
+
bool use_stability = false;
507
+
bool optimize_hand_size = true;
508
+
bool is_right = false;
509
+
float smoothing_factor;
510
+
int num_observation_views = 0;
511
+
one_frame_input *observation = nullptr;
512
+
513
+
HandScalar target_hand_size = {};
514
+
HandScalar hand_size_err_mul = {};
515
+
HandScalar depth_err_mul = {};
516
+
517
+
518
+
u_logging_level log_level = U_LOGGING_INFO;
519
+
520
+
Quat<HandScalar> last_frame_pre_rotation = {};
521
+
OptimizerHand<HandScalar> last_frame = {};
522
+
523
+
// The pose that will take you from the right camera's space to the left camera's space.
524
+
xrt_pose left_in_right = {};
525
+
526
+
// The translation part of the same pose, just easier for Ceres to consume
527
+
Vec3<HandScalar> left_in_right_translation = {};
528
+
529
+
// The orientation part of the same pose, just easier for Ceres to consume
530
+
Quat<HandScalar> left_in_right_orientation = {};
531
+
532
+
Eigen::Matrix<HandScalar, calc_input_size(true), 1> TinyOptimizerInput = {};
533
+
};
534
+
535
+
template <typename T> struct Translations55
536
+
{
537
+
Vec3<T> t[kNumFingers][kNumJointsInFinger] = {};
538
+
};
539
+
540
+
template <typename T> struct Orientations54
541
+
{
542
+
Quat<T> q[kNumFingers][kNumJointsInFinger] = {};
543
+
};
544
+
545
+
template <bool optimize_hand_size> struct CostFunctor
546
+
{
547
+
KinematicHandLM &parent;
548
+
size_t num_residuals_;
549
+
550
+
template <typename T>
551
+
bool
552
+
operator()(const T *const x, T *residual) const;
553
+
554
+
CostFunctor(KinematicHandLM &in_last_hand, size_t const &num_residuals)
555
+
: parent(in_last_hand), num_residuals_(num_residuals)
556
+
{}
557
+
558
+
size_t
559
+
NumResiduals() const
560
+
{
561
+
return num_residuals_;
562
+
}
563
+
};
564
+
565
+
360
566
} // namespace xrt::tracking::hand::mercury::lm
-89
src/xrt/tracking/hand/mercury/kine_lm/lm_main.cpp
-89
src/xrt/tracking/hand/mercury/kine_lm/lm_main.cpp
···
11
11
#include "math/m_api.h"
12
12
#include "math/m_vec3.h"
13
13
#include "os/os_time.h"
14
-
#include "util/u_logging.h"
15
14
#include "util/u_misc.h"
16
15
#include "util/u_trace_marker.h"
17
16
···
36
35
*/
37
36
38
37
namespace xrt::tracking::hand::mercury::lm {
39
-
40
-
template <typename T> struct StereographicObservation
41
-
{
42
-
Vec2<T> obs[kNumNNJoints];
43
-
};
44
-
45
-
46
-
template <typename T> struct DepthObservation
47
-
{
48
-
T depth_value[kNumNNJoints];
49
-
};
50
-
51
-
template <typename T> struct ResidualTracker
52
-
{
53
-
T *out_residual = nullptr;
54
-
size_t out_residual_idx = {};
55
-
56
-
ResidualTracker(T *residual) : out_residual(residual) {}
57
-
58
-
void
59
-
AddValue(T const &value)
60
-
{
61
-
this->out_residual[out_residual_idx++] = value;
62
-
}
63
-
};
64
-
65
-
66
-
struct KinematicHandLM
67
-
{
68
-
bool first_frame = true;
69
-
bool use_stability = false;
70
-
bool optimize_hand_size = true;
71
-
bool is_right = false;
72
-
float smoothing_factor;
73
-
int num_observation_views = 0;
74
-
one_frame_input *observation = nullptr;
75
-
76
-
HandScalar target_hand_size = {};
77
-
HandScalar hand_size_err_mul = {};
78
-
HandScalar depth_err_mul = {};
79
-
80
-
81
-
u_logging_level log_level = U_LOGGING_INFO;
82
-
83
-
Quat<HandScalar> last_frame_pre_rotation = {};
84
-
OptimizerHand<HandScalar> last_frame = {};
85
-
86
-
// The pose that will take you from the right camera's space to the left camera's space.
87
-
xrt_pose left_in_right = {};
88
-
89
-
// The translation part of the same pose, just easier for Ceres to consume
90
-
Vec3<HandScalar> left_in_right_translation = {};
91
-
92
-
// The orientation part of the same pose, just easier for Ceres to consume
93
-
Quat<HandScalar> left_in_right_orientation = {};
94
-
95
-
Eigen::Matrix<HandScalar, calc_input_size(true), 1> TinyOptimizerInput = {};
96
-
};
97
-
98
-
template <typename T> struct Translations55
99
-
{
100
-
Vec3<T> t[kNumFingers][kNumJointsInFinger] = {};
101
-
};
102
-
103
-
template <typename T> struct Orientations54
104
-
{
105
-
Quat<T> q[kNumFingers][kNumJointsInFinger] = {};
106
-
};
107
-
108
-
template <bool optimize_hand_size> struct CostFunctor
109
-
{
110
-
KinematicHandLM &parent;
111
-
size_t num_residuals_;
112
-
113
-
template <typename T>
114
-
bool
115
-
operator()(const T *const x, T *residual) const;
116
-
117
-
CostFunctor(KinematicHandLM &in_last_hand, size_t const &num_residuals)
118
-
: parent(in_last_hand), num_residuals_(num_residuals)
119
-
{}
120
-
121
-
size_t
122
-
NumResiduals() const
123
-
{
124
-
return num_residuals_;
125
-
}
126
-
};
127
38
128
39
template <typename T>
129
40
static inline void
-110
src/xrt/tracking/hand/mercury/kine_lm/lm_optimizer_params_packer.inl
-110
src/xrt/tracking/hand/mercury/kine_lm/lm_optimizer_params_packer.inl
···
18
18
19
19
namespace xrt::tracking::hand::mercury::lm {
20
20
21
-
template <typename T> struct OptimizerMetacarpalBone
22
-
{
23
-
Vec2<T> swing = {};
24
-
T twist = {};
25
-
};
26
-
27
-
template <typename T> struct OptimizerFinger
28
-
{
29
-
OptimizerMetacarpalBone<T> metacarpal = {};
30
-
Vec2<T> proximal_swing = {};
31
-
// Not Vec2.
32
-
T rots[2] = {};
33
-
};
34
-
35
-
template <typename T> struct OptimizerThumb
36
-
{
37
-
OptimizerMetacarpalBone<T> metacarpal = {};
38
-
// Again not Vec2.
39
-
T rots[2] = {};
40
-
};
41
-
42
-
template <typename T> struct OptimizerHand
43
-
{
44
-
T hand_size;
45
-
Vec3<T> wrist_location = {};
46
-
// This is constant, a ceres::Rotation.h quat,, taken from last frame.
47
-
Quat<T> wrist_pre_orientation_quat = {};
48
-
// This is optimized - angle-axis rotation vector. Starts at 0, loss goes up the higher it goes because it
49
-
// indicates more of a rotation.
50
-
Vec3<T> wrist_post_orientation_aax = {};
51
-
OptimizerThumb<T> thumb = {};
52
-
OptimizerFinger<T> finger[4] = {};
53
-
};
54
-
55
-
56
-
struct minmax
57
-
{
58
-
HandScalar min = 0;
59
-
HandScalar max = 0;
60
-
};
61
-
62
-
class FingerLimit
63
-
{
64
-
public:
65
-
minmax mcp_swing_x = {};
66
-
minmax mcp_swing_y = {};
67
-
minmax mcp_twist = {};
68
-
69
-
minmax pxm_swing_x = {};
70
-
minmax pxm_swing_y = {};
71
-
72
-
minmax curls[2] = {}; // int, dst
73
-
};
74
-
75
-
class HandLimit
76
-
{
77
-
public:
78
-
minmax hand_size = {};
79
-
80
-
minmax thumb_mcp_swing_x = {};
81
-
minmax thumb_mcp_swing_y = {};
82
-
minmax thumb_mcp_twist = {};
83
-
minmax thumb_curls[2] = {};
84
-
85
-
FingerLimit fingers[4] = {};
86
-
87
-
HandLimit()
88
-
{
89
-
hand_size = {MIN_HAND_SIZE, MAX_HAND_SIZE};
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
-
HandScalar margin = 0.0001;
101
-
102
-
fingers[0].mcp_swing_y = {HandScalar(-0.19) - margin, HandScalar(-0.19) + margin};
103
-
fingers[1].mcp_swing_y = {HandScalar(0.00) - margin, HandScalar(0.00) + margin};
104
-
fingers[2].mcp_swing_y = {HandScalar(0.19) - margin, HandScalar(0.19) + margin};
105
-
fingers[3].mcp_swing_y = {HandScalar(0.38) - margin, HandScalar(0.38) + margin};
106
-
107
-
108
-
fingers[0].mcp_swing_x = {HandScalar(-0.02) - margin, HandScalar(-0.02) + margin};
109
-
fingers[1].mcp_swing_x = {HandScalar(0.00) - margin, HandScalar(0.00) + margin};
110
-
fingers[2].mcp_swing_x = {HandScalar(0.02) - margin, HandScalar(0.02) + margin};
111
-
fingers[3].mcp_swing_x = {HandScalar(0.04) - margin, HandScalar(0.04) + margin};
112
-
113
-
114
-
for (int finger_idx = 0; finger_idx < 4; finger_idx++) {
115
-
FingerLimit &finger = fingers[finger_idx];
116
-
117
-
// finger.mcp_swing_x = {rad<HandScalar>(-0.0001), rad<HandScalar>(0.0001)};
118
-
finger.mcp_twist = {rad<HandScalar>(-4), rad<HandScalar>(4)};
119
-
120
-
finger.pxm_swing_x = {rad<HandScalar>(-100), rad<HandScalar>(20)}; // ??? why is it reversed
121
-
finger.pxm_swing_y = {rad<HandScalar>(-20), rad<HandScalar>(20)};
122
-
123
-
for (int i = 0; i < 2; i++) {
124
-
finger.curls[i] = {rad<HandScalar>(-90), rad<HandScalar>(0)};
125
-
}
126
-
}
127
-
}
128
-
};
129
-
130
-
static const class HandLimit the_limit = {};
131
21
132
22
133
23
template <typename T>