The open source OpenXR runtime
Revert "a/math: Add filtered motion vectors to relation history"
See https://gitlab.freedesktop.org/monado/monado/-/issues/528
This reverts commit 339a2b536eff5d7682d3d9c41cb121b2da87316d.
Part-of: <https://gitlab.freedesktop.org/monado/monado/-/merge_requests/2495>
+38
-90
+16
-60
src/xrt/auxiliary/math/m_relation_history.cpp
+16
-60
src/xrt/auxiliary/math/m_relation_history.cpp
···
17
17
#include "math/m_api.h"
18
18
#include "math/m_predict.h"
19
19
#include "math/m_vec3.h"
20
-
#include "math/m_filter_one_euro.h"
21
20
22
21
#include "os/os_time.h"
23
22
#include "os/os_threading.h"
···
48
47
49
48
struct m_relation_history
50
49
{
51
-
mutable os::Mutex mutex;
52
-
53
50
HistoryBuffer<struct relation_history_entry, BufLen> impl;
54
-
55
-
struct m_relation_history_filters *motion_vector_filters;
51
+
mutable os::Mutex mutex;
56
52
};
53
+
57
54
58
55
void
59
-
m_relation_history_create(struct m_relation_history **rh_ptr, struct m_relation_history_filters *motion_vector_filters)
56
+
m_relation_history_create(struct m_relation_history **rh_ptr)
60
57
{
61
58
auto ret = std::make_unique<m_relation_history>();
62
-
63
-
ret->motion_vector_filters = motion_vector_filters;
64
-
65
59
*rh_ptr = ret.release();
66
60
}
67
61
···
195
189
int64_t timestamp,
196
190
struct xrt_space_relation *out_relation)
197
191
{
192
+
198
193
int64_t last_time_ns;
199
194
struct xrt_space_relation last_relation;
200
195
if (!m_relation_history_get_latest(rh, &last_time_ns, &last_relation)) {
···
210
205
// Brevity
211
206
enum xrt_space_relation_flags &outf = out_relation->relation_flags;
212
207
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;
217
-
218
-
struct xrt_vec3 new_position;
219
-
struct xrt_quat new_orientation;
220
208
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
-
}
225
-
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
-
}
230
-
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);
236
-
237
-
outf = (enum xrt_space_relation_flags)(outf | XRT_SPACE_RELATION_LINEAR_VELOCITY_VALID_BIT);
238
-
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);
209
+
if (tmp_flags & XRT_SPACE_RELATION_POSITION_VALID_BIT) {
210
+
outf = (enum xrt_space_relation_flags)(outf | XRT_SPACE_RELATION_POSITION_VALID_BIT);
211
+
outf = (enum xrt_space_relation_flags)(outf | XRT_SPACE_RELATION_POSITION_TRACKED_BIT);
255
212
256
-
outf = (enum xrt_space_relation_flags)(outf | XRT_SPACE_RELATION_LINEAR_VELOCITY_VALID_BIT);
213
+
outf = (enum xrt_space_relation_flags)(outf | XRT_SPACE_RELATION_LINEAR_VELOCITY_VALID_BIT);
257
214
258
-
out_relation->linear_velocity = (in_relation->pose.position - last_relation.pose.position) / dt;
259
-
}
215
+
out_relation->linear_velocity = (in_relation->pose.position - last_relation.pose.position) / dt;
216
+
}
260
217
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);
218
+
if (tmp_flags & XRT_SPACE_RELATION_ORIENTATION_VALID_BIT) {
219
+
outf = (enum xrt_space_relation_flags)(outf | XRT_SPACE_RELATION_ORIENTATION_VALID_BIT);
220
+
outf = (enum xrt_space_relation_flags)(outf | XRT_SPACE_RELATION_ORIENTATION_TRACKED_BIT);
264
221
265
-
outf = (enum xrt_space_relation_flags)(outf | XRT_SPACE_RELATION_ANGULAR_VELOCITY_VALID_BIT);
222
+
outf = (enum xrt_space_relation_flags)(outf | XRT_SPACE_RELATION_ANGULAR_VELOCITY_VALID_BIT);
266
223
267
-
math_quat_finite_difference(&last_relation.pose.orientation, &in_relation->pose.orientation, dt,
268
-
&out_relation->angular_velocity);
269
-
}
224
+
math_quat_finite_difference(&last_relation.pose.orientation, &in_relation->pose.orientation, dt,
225
+
&out_relation->angular_velocity);
270
226
}
271
227
272
228
out_relation->pose = in_relation->pose;
+2
-10
src/xrt/auxiliary/math/m_relation_history.h
+2
-10
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
-
18
16
#ifdef __cplusplus
19
17
extern "C" {
20
18
#endif
···
44
42
M_RELATION_HISTORY_RESULT_REVERSE_PREDICTED, //!< The desired timestamp was older than the oldest entry
45
43
};
46
44
47
-
struct m_relation_history_filters
48
-
{
49
-
struct m_filter_euro_vec3 position;
50
-
struct m_filter_euro_quat orientation;
51
-
};
52
-
53
45
/*!
54
46
* Creates an opaque relation_history object.
55
47
*
56
48
* @public @memberof m_relation_history
57
49
*/
58
50
void
59
-
m_relation_history_create(struct m_relation_history **rh, struct m_relation_history_filters *motion_vector_filters);
51
+
m_relation_history_create(struct m_relation_history **rh);
60
52
61
53
/*!
62
54
* Pushes a new pose to the history.
···
167
159
168
160
public:
169
161
// clang-format off
170
-
RelationHistory(struct m_relation_history_filters *motion_vector_filters) noexcept { m_relation_history_create(&mPtr, motion_vector_filters); }
162
+
RelationHistory() noexcept { m_relation_history_create(&mPtr); }
171
163
~RelationHistory() { m_relation_history_destroy(&mPtr); }
172
164
// clang-format on
173
165
+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{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
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
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, NULL);
60
+
m_relation_history_create(&dof3->rh);
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
···
432
432
{
433
433
struct rs_ddev *rs = U_DEVICE_ALLOCATE(struct rs_ddev, U_DEVICE_ALLOC_TRACKING_NONE, 1, 0);
434
434
435
-
m_relation_history_create(&rs->relation_hist, NULL);
435
+
m_relation_history_create(&rs->relation_hist);
436
436
437
437
rs->enable_mapping = true;
438
438
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, NULL);
198
+
m_relation_history_create(&hmd->relation_hist);
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
···
197
197
198
198
Device::Device(const DeviceBuilder &builder) : xrt_device({}), ctx(builder.ctx), driver(builder.driver)
199
199
{
200
-
m_relation_history_create(&relation_hist, NULL);
200
+
m_relation_history_create(&relation_hist);
201
201
std::strncpy(this->serial, builder.serial, XRT_DEVICE_NAME_LEN - 1);
202
202
this->serial[XRT_DEVICE_NAME_LEN - 1] = 0;
203
203
this->tracking_origin = ctx.get();
+2
-2
src/xrt/drivers/survive/survive_driver.c
+2
-2
src/xrt/drivers/survive/survive_driver.c
···
944
944
survive->base.tracking_origin = &sys->base;
945
945
946
946
SURVIVE_INFO(survive, "survive HMD present");
947
-
m_relation_history_create(&survive->relation_hist, NULL);
947
+
m_relation_history_create(&survive->relation_hist);
948
948
949
949
950
950
size_t idx = 0;
···
1093
1093
int outputs = 1;
1094
1094
struct survive_device *survive = U_DEVICE_ALLOCATE(struct survive_device, flags, inputs, outputs);
1095
1095
survive->ctrl.config = *config;
1096
-
m_relation_history_create(&survive->relation_hist, NULL);
1096
+
m_relation_history_create(&survive->relation_hist);
1097
1097
1098
1098
sys->controllers[idx] = survive;
1099
1099
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, NULL);
1066
+
m_relation_history_create(&d->fusion.relation_hist);
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
···
1081
1081
(enum u_device_alloc_flags)(U_DEVICE_ALLOC_HMD | U_DEVICE_ALLOC_TRACKING_NONE);
1082
1082
struct vive_device *d = U_DEVICE_ALLOCATE(struct vive_device, flags, 1, 0);
1083
1083
1084
-
m_relation_history_create(&d->fusion.relation_hist, NULL);
1084
+
m_relation_history_create(&d->fusion.relation_hist);
1085
1085
1086
1086
size_t idx = 0;
1087
1087
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
···
1210
1210
u_distortion_mesh_set_none(&hmd->base);
1211
1211
1212
1212
m_imu_3dof_init(&hmd->fusion, M_IMU_3DOF_USE_GRAVITY_DUR_20MS);
1213
-
m_relation_history_create(&hmd->relation_hist, NULL);
1213
+
m_relation_history_create(&hmd->relation_hist);
1214
1214
1215
1215
hmd->static_id = 0;
1216
1216
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, nullptr);
34
+
m_relation_history_create(&rh);
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{nullptr};
128
+
RelationHistory rh;
129
129
130
130
SECTION("empty buffer")
131
131
{