The open source OpenXR runtime
a/math: Add filtered motion vectors to relation history
The one-euro parameters were hand-tuned for what "felt okay" for the razer hydra specifically
Part-of: <https://gitlab.freedesktop.org/monado/monado/-/merge_requests/2471>
+95
-40
+65
-18
src/xrt/auxiliary/math/m_relation_history.cpp
+65
-18
src/xrt/auxiliary/math/m_relation_history.cpp
···
12
12
13
13
#include "m_relation_history.h"
14
14
15
+
#include "xrt/xrt_defines.h"
16
+
15
17
#include "math/m_api.h"
16
18
#include "math/m_predict.h"
17
19
#include "math/m_vec3.h"
20
+
#include "math/m_filter_one_euro.h"
21
+
18
22
#include "os/os_time.h"
23
+
#include "os/os_threading.h"
24
+
19
25
#include "util/u_logging.h"
20
26
#include "util/u_trace_marker.h"
21
-
#include "xrt/xrt_defines.h"
22
-
#include "os/os_threading.h"
23
27
#include "util/u_template_historybuf.hpp"
24
28
25
29
#include <memory>
···
44
48
45
49
struct m_relation_history
46
50
{
51
+
mutable os::Mutex mutex;
52
+
47
53
HistoryBuffer<struct relation_history_entry, BufLen> impl;
48
-
mutable os::Mutex mutex;
54
+
55
+
struct m_relation_history_filters *motion_vector_filters;
49
56
};
50
57
51
-
52
58
void
53
-
m_relation_history_create(struct m_relation_history **rh_ptr)
59
+
m_relation_history_create(struct m_relation_history **rh_ptr, struct m_relation_history_filters *motion_vector_filters)
54
60
{
55
61
auto ret = std::make_unique<m_relation_history>();
62
+
63
+
ret->motion_vector_filters = motion_vector_filters;
64
+
56
65
*rh_ptr = ret.release();
57
66
}
58
67
···
186
195
int64_t timestamp,
187
196
struct xrt_space_relation *out_relation)
188
197
{
189
-
190
198
int64_t last_time_ns;
191
199
struct xrt_space_relation last_relation;
192
200
if (!m_relation_history_get_latest(rh, &last_time_ns, &last_relation)) {
···
202
210
// Brevity
203
211
enum xrt_space_relation_flags &outf = out_relation->relation_flags;
204
212
213
+
// update the filters if we're doing filtered motion vectors
214
+
if (rh->motion_vector_filters) {
215
+
struct xrt_vec3 previous_position = rh->motion_vector_filters->position.prev_y;
216
+
struct xrt_quat previous_orientation = rh->motion_vector_filters->orientation.prev_y;
205
217
206
-
if (tmp_flags & XRT_SPACE_RELATION_POSITION_VALID_BIT) {
207
-
outf = (enum xrt_space_relation_flags)(outf | XRT_SPACE_RELATION_POSITION_VALID_BIT);
208
-
outf = (enum xrt_space_relation_flags)(outf | XRT_SPACE_RELATION_POSITION_TRACKED_BIT);
218
+
struct xrt_vec3 new_position;
219
+
struct xrt_quat new_orientation;
209
220
210
-
outf = (enum xrt_space_relation_flags)(outf | XRT_SPACE_RELATION_LINEAR_VELOCITY_VALID_BIT);
221
+
if (in_relation->relation_flags & XRT_SPACE_RELATION_POSITION_VALID_BIT) {
222
+
m_filter_euro_vec3_run(&rh->motion_vector_filters->position, timestamp,
223
+
&in_relation->pose.position, &new_position);
224
+
}
211
225
212
-
out_relation->linear_velocity = (in_relation->pose.position - last_relation.pose.position) / dt;
213
-
}
226
+
if (in_relation->relation_flags & XRT_SPACE_RELATION_ORIENTATION_VALID_BIT) {
227
+
m_filter_euro_quat_run(&rh->motion_vector_filters->orientation, timestamp,
228
+
&in_relation->pose.orientation, &new_orientation);
229
+
}
214
230
215
-
if (tmp_flags & XRT_SPACE_RELATION_ORIENTATION_VALID_BIT) {
216
-
outf = (enum xrt_space_relation_flags)(outf | XRT_SPACE_RELATION_ORIENTATION_VALID_BIT);
217
-
outf = (enum xrt_space_relation_flags)(outf | XRT_SPACE_RELATION_ORIENTATION_TRACKED_BIT);
231
+
// @note this looks subtly wrong, where technically in valid sequences of relations
232
+
// (valid, invalid, valid) we won't produce a filtered pose, but this is intentional.
233
+
if (tmp_flags & XRT_SPACE_RELATION_POSITION_VALID_BIT) {
234
+
outf = (enum xrt_space_relation_flags)(outf | XRT_SPACE_RELATION_POSITION_VALID_BIT);
235
+
outf = (enum xrt_space_relation_flags)(outf | XRT_SPACE_RELATION_POSITION_TRACKED_BIT);
218
236
219
-
outf = (enum xrt_space_relation_flags)(outf | XRT_SPACE_RELATION_ANGULAR_VELOCITY_VALID_BIT);
237
+
outf = (enum xrt_space_relation_flags)(outf | XRT_SPACE_RELATION_LINEAR_VELOCITY_VALID_BIT);
220
238
221
-
math_quat_finite_difference(&last_relation.pose.orientation, &in_relation->pose.orientation, dt,
222
-
&out_relation->angular_velocity);
239
+
out_relation->linear_velocity = (new_position - previous_position) / dt;
240
+
}
241
+
242
+
if (tmp_flags & XRT_SPACE_RELATION_ORIENTATION_VALID_BIT) {
243
+
outf = (enum xrt_space_relation_flags)(outf | XRT_SPACE_RELATION_ORIENTATION_VALID_BIT);
244
+
outf = (enum xrt_space_relation_flags)(outf | XRT_SPACE_RELATION_ORIENTATION_TRACKED_BIT);
245
+
246
+
outf = (enum xrt_space_relation_flags)(outf | XRT_SPACE_RELATION_ANGULAR_VELOCITY_VALID_BIT);
247
+
248
+
math_quat_finite_difference(&previous_orientation, &new_orientation, dt,
249
+
&out_relation->angular_velocity);
250
+
}
251
+
} else {
252
+
if (tmp_flags & XRT_SPACE_RELATION_POSITION_VALID_BIT) {
253
+
outf = (enum xrt_space_relation_flags)(outf | XRT_SPACE_RELATION_POSITION_VALID_BIT);
254
+
outf = (enum xrt_space_relation_flags)(outf | XRT_SPACE_RELATION_POSITION_TRACKED_BIT);
255
+
256
+
outf = (enum xrt_space_relation_flags)(outf | XRT_SPACE_RELATION_LINEAR_VELOCITY_VALID_BIT);
257
+
258
+
out_relation->linear_velocity = (in_relation->pose.position - last_relation.pose.position) / dt;
259
+
}
260
+
261
+
if (tmp_flags & XRT_SPACE_RELATION_ORIENTATION_VALID_BIT) {
262
+
outf = (enum xrt_space_relation_flags)(outf | XRT_SPACE_RELATION_ORIENTATION_VALID_BIT);
263
+
outf = (enum xrt_space_relation_flags)(outf | XRT_SPACE_RELATION_ORIENTATION_TRACKED_BIT);
264
+
265
+
outf = (enum xrt_space_relation_flags)(outf | XRT_SPACE_RELATION_ANGULAR_VELOCITY_VALID_BIT);
266
+
267
+
math_quat_finite_difference(&last_relation.pose.orientation, &in_relation->pose.orientation, dt,
268
+
&out_relation->angular_velocity);
269
+
}
223
270
}
224
271
225
272
out_relation->pose = in_relation->pose;
+10
-2
src/xrt/auxiliary/math/m_relation_history.h
+10
-2
src/xrt/auxiliary/math/m_relation_history.h
···
13
13
14
14
#include "xrt/xrt_defines.h"
15
15
16
+
#include "math/m_filter_one_euro.h"
17
+
16
18
#ifdef __cplusplus
17
19
extern "C" {
18
20
#endif
···
42
44
M_RELATION_HISTORY_RESULT_REVERSE_PREDICTED, //!< The desired timestamp was older than the oldest entry
43
45
};
44
46
47
+
struct m_relation_history_filters
48
+
{
49
+
struct m_filter_euro_vec3 position;
50
+
struct m_filter_euro_quat orientation;
51
+
};
52
+
45
53
/*!
46
54
* Creates an opaque relation_history object.
47
55
*
48
56
* @public @memberof m_relation_history
49
57
*/
50
58
void
51
-
m_relation_history_create(struct m_relation_history **rh);
59
+
m_relation_history_create(struct m_relation_history **rh, struct m_relation_history_filters *motion_vector_filters);
52
60
53
61
/*!
54
62
* Pushes a new pose to the history.
···
159
167
160
168
public:
161
169
// clang-format off
162
-
RelationHistory() noexcept { m_relation_history_create(&mPtr); }
170
+
RelationHistory(struct m_relation_history_filters *motion_vector_filters) noexcept { m_relation_history_create(&mPtr, motion_vector_filters); }
163
171
~RelationHistory() { m_relation_history_destroy(&mPtr); }
164
172
// clang-format on
165
173
+8
-8
src/xrt/auxiliary/tracking/t_tracker_slam.cpp
+8
-8
src/xrt/auxiliary/tracking/t_tracker_slam.cpp
···
286
286
287
287
//! Type of prediction to use
288
288
t_slam_prediction_type pred_type;
289
-
u_var_combo pred_combo; //!< UI combo box to select @ref pred_type
290
-
RelationHistory slam_rels{}; //!< A history of relations produced purely from external SLAM tracker data
291
-
int dbg_pred_every = 1; //!< Skip X SLAM poses so that you get tracked mostly by the prediction algo
292
-
int dbg_pred_counter = 0; //!< SLAM pose counter for prediction debugging
293
-
struct os_mutex lock_ff; //!< Lock for gyro_ff and accel_ff.
294
-
struct m_ff_vec3_f32 *gyro_ff; //!< Last gyroscope samples
295
-
struct m_ff_vec3_f32 *accel_ff; //!< Last accelerometer samples
296
-
vector<u_sink_debug> ui_sink; //!< Sink to display frames in UI of each camera
289
+
u_var_combo pred_combo; //!< UI combo box to select @ref pred_type
290
+
RelationHistory slam_rels{nullptr}; //!< A history of relations produced purely from external SLAM tracker data
291
+
int dbg_pred_every = 1; //!< Skip X SLAM poses so that you get tracked mostly by the prediction algo
292
+
int dbg_pred_counter = 0; //!< SLAM pose counter for prediction debugging
293
+
struct os_mutex lock_ff; //!< Lock for gyro_ff and accel_ff.
294
+
struct m_ff_vec3_f32 *gyro_ff; //!< Last gyroscope samples
295
+
struct m_ff_vec3_f32 *accel_ff; //!< Last accelerometer samples
296
+
vector<u_sink_debug> ui_sink; //!< Sink to display frames in UI of each camera
297
297
298
298
//! Used to correct accelerometer measurements when integrating into the prediction.
299
299
//! @todo Should be automatically computed instead of required to be filled manually through the UI.
+1
-1
src/xrt/auxiliary/util/u_tracked_imu_3dof.c
+1
-1
src/xrt/auxiliary/util/u_tracked_imu_3dof.c
···
57
57
{
58
58
struct u_tracked_imu_3dof *dof3 = U_TYPED_CALLOC(struct u_tracked_imu_3dof);
59
59
60
-
m_relation_history_create(&dof3->rh);
60
+
m_relation_history_create(&dof3->rh, NULL);
61
61
62
62
m_imu_3dof_init(&dof3->fusion, M_IMU_3DOF_USE_GRAVITY_DUR_300MS);
63
63
m_imu_3dof_add_vars(&dof3->fusion, debug_var_root, "");
+1
-1
src/xrt/drivers/realsense/rs_ddev.c
+1
-1
src/xrt/drivers/realsense/rs_ddev.c
···
444
444
{
445
445
struct rs_ddev *rs = U_DEVICE_ALLOCATE(struct rs_ddev, U_DEVICE_ALLOC_TRACKING_NONE, 1, 0);
446
446
447
-
m_relation_history_create(&rs->relation_hist);
447
+
m_relation_history_create(&rs->relation_hist, NULL);
448
448
449
449
rs->enable_mapping = true;
450
450
rs->enable_pose_jumping = true;
+1
-1
src/xrt/drivers/sample/sample_hmd.c
+1
-1
src/xrt/drivers/sample/sample_hmd.c
···
195
195
snprintf(hmd->base.str, XRT_DEVICE_NAME_LEN, "Sample HMD");
196
196
snprintf(hmd->base.serial, XRT_DEVICE_NAME_LEN, "Sample HMD S/N");
197
197
198
-
m_relation_history_create(&hmd->relation_hist);
198
+
m_relation_history_create(&hmd->relation_hist, NULL);
199
199
200
200
// Setup input.
201
201
hmd->base.name = XRT_DEVICE_GENERIC_HMD;
+1
-1
src/xrt/drivers/steamvr_lh/device.cpp
+1
-1
src/xrt/drivers/steamvr_lh/device.cpp
···
198
198
199
199
Device::Device(const DeviceBuilder &builder) : xrt_device({}), ctx(builder.ctx), driver(builder.driver)
200
200
{
201
-
m_relation_history_create(&relation_hist);
201
+
m_relation_history_create(&relation_hist, NULL);
202
202
std::strncpy(this->serial, builder.serial, XRT_DEVICE_NAME_LEN - 1);
203
203
this->serial[XRT_DEVICE_NAME_LEN - 1] = 0;
204
204
this->tracking_origin = ctx.get();
+2
-2
src/xrt/drivers/survive/survive_driver.c
+2
-2
src/xrt/drivers/survive/survive_driver.c
···
934
934
survive->base.tracking_origin = &sys->base;
935
935
936
936
SURVIVE_INFO(survive, "survive HMD present");
937
-
m_relation_history_create(&survive->relation_hist);
937
+
m_relation_history_create(&survive->relation_hist, NULL);
938
938
939
939
940
940
size_t idx = 0;
···
1083
1083
int outputs = 1;
1084
1084
struct survive_device *survive = U_DEVICE_ALLOCATE(struct survive_device, flags, inputs, outputs);
1085
1085
survive->ctrl.config = *config;
1086
-
m_relation_history_create(&survive->relation_hist);
1086
+
m_relation_history_create(&survive->relation_hist, NULL);
1087
1087
1088
1088
sys->controllers[idx] = survive;
1089
1089
survive->sys = sys;
+1
-1
src/xrt/drivers/vive/vive_controller.c
+1
-1
src/xrt/drivers/vive/vive_controller.c
···
1063
1063
d->watchman_gen = watchman_gen;
1064
1064
1065
1065
m_imu_3dof_init(&d->fusion.i3dof, M_IMU_3DOF_USE_GRAVITY_DUR_20MS);
1066
-
m_relation_history_create(&d->fusion.relation_hist);
1066
+
m_relation_history_create(&d->fusion.relation_hist, NULL);
1067
1067
int ret = os_mutex_init(&d->fusion.mutex);
1068
1068
if (ret != 0) {
1069
1069
VIVE_ERROR(d, "Failed to init 3dof mutex");
+1
-1
src/xrt/drivers/vive/vive_device.c
+1
-1
src/xrt/drivers/vive/vive_device.c
···
1076
1076
(enum u_device_alloc_flags)(U_DEVICE_ALLOC_HMD | U_DEVICE_ALLOC_TRACKING_NONE);
1077
1077
struct vive_device *d = U_DEVICE_ALLOCATE(struct vive_device, flags, 1, 0);
1078
1078
1079
-
m_relation_history_create(&d->fusion.relation_hist);
1079
+
m_relation_history_create(&d->fusion.relation_hist, NULL);
1080
1080
1081
1081
size_t idx = 0;
1082
1082
d->base.hmd->blend_modes[idx++] = XRT_BLEND_MODE_OPAQUE;
+1
-1
src/xrt/drivers/xreal_air/xreal_air_hmd.c
+1
-1
src/xrt/drivers/xreal_air/xreal_air_hmd.c
···
1130
1130
u_distortion_mesh_set_none(&hmd->base);
1131
1131
1132
1132
m_imu_3dof_init(&hmd->fusion, M_IMU_3DOF_USE_GRAVITY_DUR_20MS);
1133
-
m_relation_history_create(&hmd->relation_hist);
1133
+
m_relation_history_create(&hmd->relation_hist, NULL);
1134
1134
1135
1135
hmd->static_id = 0;
1136
1136
hmd->display_on = false;
+1
-1
src/xrt/tracking/hand/t_hand_tracking_async.c
+1
-1
src/xrt/tracking/hand/t_hand_tracking_async.c
+2
-2
tests/tests_history_buf.cpp
+2
-2
tests/tests_history_buf.cpp
···
31
31
{
32
32
m_relation_history *rh = nullptr;
33
33
34
-
m_relation_history_create(&rh);
34
+
m_relation_history_create(&rh, nullptr);
35
35
SECTION("empty buffer")
36
36
{
37
37
xrt_space_relation out_relation = XRT_SPACE_RELATION_ZERO;
···
125
125
TEST_CASE("RelationHistory")
126
126
{
127
127
using xrt::auxiliary::math::RelationHistory;
128
-
RelationHistory rh;
128
+
RelationHistory rh{nullptr};
129
129
130
130
SECTION("empty buffer")
131
131
{