The open source OpenXR runtime
matrixfurry.com
1// Copyright 2022, Collabora, Inc.
2// SPDX-License-Identifier: BSL-1.0
3/*!
4 * @file
5 * @brief Test for Levenberg-Marquardt kinematic optimizer
6 * @author Moshi Turner <moshiturner@protonmail.com>
7 * @author Rylie Pavlik <rylie.pavlik@collabora.com>
8 */
9#include "util/u_logging.h"
10#include "xrt/xrt_defines.h"
11#include <util/u_worker.hpp>
12#include <math/m_mathinclude.h>
13#include <math/m_space.h>
14#include <math/m_vec3.h>
15#include <math/m_vec2.h>
16
17#include "kine_common.hpp"
18#include "lm_interface.hpp"
19
20#include "catch_amalgamated.hpp"
21
22#include <thread>
23#include <chrono>
24#include "fenv.h"
25
26using namespace xrt::tracking::hand::mercury;
27
28TEST_CASE("LevenbergMarquardt")
29{
30 // This does very little at the moment:
31 // * It will explode if any floating point exceptions are generated
32 // * You should run it with `valgrind --track-origins=yes` (and compile without optimizations so that origin
33 // tracking works well) to see if we are using any uninitialized values.
34
35 fetestexcept(FE_ALL_EXCEPT);
36
37 struct one_frame_input input = {};
38
39 for (int view = 0; view < 2; view++) {
40 input.views[view].active = true;
41 input.views[view].stereographic_radius = 0.5;
42 input.views[view].look_dir = XRT_QUAT_IDENTITY;
43 for (int i = 0; i < 5; i++) {
44
45 input.views[view].curls[i].value = -0.5f;
46 input.views[view].curls[i].variance = 1.0f;
47 }
48 for (int i = 0; i < 21; i++) {
49
50 xrt_vec2 dir = {sinf(i), cosf(i)};
51 m_vec2_normalize(&dir);
52
53 input.views[view].keypoints_in_scaled_stereographic[i].pos_2d = dir; //{0,(float)i,-1};
54 input.views[view].keypoints_in_scaled_stereographic[i].depth_relative_to_midpxm =
55 (i / 21.0f) - 0.5;
56 input.views[view].keypoints_in_scaled_stereographic[i].confidence_depth = 1.0f;
57 input.views[view].keypoints_in_scaled_stereographic[i].confidence_xy = 1.0f;
58 }
59 }
60
61 lm::KinematicHandLM *hand;
62
63 xrt_pose left_in_right = XRT_POSE_IDENTITY;
64 left_in_right.position.x = 1;
65
66 lm::optimizer_create(left_in_right, false, U_LOGGING_TRACE, &hand);
67
68
69 xrt_hand_joint_set out = {};
70 float out_hand_size = 0.0f;
71 float out_reprojection_error = 0.0f;
72 lm::optimizer_run(hand, //
73 input, //
74 true, //
75 2.0f, //
76 true, //
77 0.09, //
78 0.5, //
79 0.5f, //
80 out, //
81 out_hand_size, //
82 out_reprojection_error);
83
84 CHECK(std::isfinite(out_reprojection_error));
85 CHECK(std::isfinite(out_hand_size));
86}