diff --git a/src/xrt/auxiliary/math/m_relation_history.cpp b/src/xrt/auxiliary/math/m_relation_history.cpp index aa2d913ca..d7bf2769e 100644 --- a/src/xrt/auxiliary/math/m_relation_history.cpp +++ b/src/xrt/auxiliary/math/m_relation_history.cpp @@ -17,7 +17,6 @@ #include "math/m_api.h" #include "math/m_predict.h" #include "math/m_vec3.h" -#include "math/m_filter_one_euro.h" #include "os/os_time.h" #include "os/os_threading.h" @@ -48,20 +47,15 @@ static constexpr size_t BufLen = 4096; struct m_relation_history { - mutable os::Mutex mutex; - HistoryBuffer impl; - - struct m_relation_history_filters *motion_vector_filters; + mutable os::Mutex mutex; }; + void -m_relation_history_create(struct m_relation_history **rh_ptr, struct m_relation_history_filters *motion_vector_filters) +m_relation_history_create(struct m_relation_history **rh_ptr) { auto ret = std::make_unique(); - - ret->motion_vector_filters = motion_vector_filters; - *rh_ptr = ret.release(); } @@ -195,6 +189,7 @@ m_relation_history_estimate_motion(struct m_relation_history *rh, int64_t timestamp, struct xrt_space_relation *out_relation) { + int64_t last_time_ns; struct xrt_space_relation last_relation; if (!m_relation_history_get_latest(rh, &last_time_ns, &last_relation)) { @@ -210,63 +205,24 @@ m_relation_history_estimate_motion(struct m_relation_history *rh, // Brevity enum xrt_space_relation_flags &outf = out_relation->relation_flags; - // update the filters if we're doing filtered motion vectors - if (rh->motion_vector_filters) { - struct xrt_vec3 previous_position = rh->motion_vector_filters->position.prev_y; - struct xrt_quat previous_orientation = rh->motion_vector_filters->orientation.prev_y; - - struct xrt_vec3 new_position; - struct xrt_quat new_orientation; - if (in_relation->relation_flags & XRT_SPACE_RELATION_POSITION_VALID_BIT) { - m_filter_euro_vec3_run(&rh->motion_vector_filters->position, timestamp, - &in_relation->pose.position, &new_position); - } - - if (in_relation->relation_flags & XRT_SPACE_RELATION_ORIENTATION_VALID_BIT) { - m_filter_euro_quat_run(&rh->motion_vector_filters->orientation, timestamp, - &in_relation->pose.orientation, &new_orientation); - } + if (tmp_flags & XRT_SPACE_RELATION_POSITION_VALID_BIT) { + outf = (enum xrt_space_relation_flags)(outf | XRT_SPACE_RELATION_POSITION_VALID_BIT); + outf = (enum xrt_space_relation_flags)(outf | XRT_SPACE_RELATION_POSITION_TRACKED_BIT); - // @note this looks subtly wrong, where technically in valid sequences of relations - // (valid, invalid, valid) we won't produce a filtered pose, but this is intentional. - if (tmp_flags & XRT_SPACE_RELATION_POSITION_VALID_BIT) { - outf = (enum xrt_space_relation_flags)(outf | XRT_SPACE_RELATION_POSITION_VALID_BIT); - outf = (enum xrt_space_relation_flags)(outf | XRT_SPACE_RELATION_POSITION_TRACKED_BIT); + outf = (enum xrt_space_relation_flags)(outf | XRT_SPACE_RELATION_LINEAR_VELOCITY_VALID_BIT); - outf = (enum xrt_space_relation_flags)(outf | XRT_SPACE_RELATION_LINEAR_VELOCITY_VALID_BIT); - - out_relation->linear_velocity = (new_position - previous_position) / dt; - } - - if (tmp_flags & XRT_SPACE_RELATION_ORIENTATION_VALID_BIT) { - outf = (enum xrt_space_relation_flags)(outf | XRT_SPACE_RELATION_ORIENTATION_VALID_BIT); - outf = (enum xrt_space_relation_flags)(outf | XRT_SPACE_RELATION_ORIENTATION_TRACKED_BIT); - - outf = (enum xrt_space_relation_flags)(outf | XRT_SPACE_RELATION_ANGULAR_VELOCITY_VALID_BIT); - - math_quat_finite_difference(&previous_orientation, &new_orientation, dt, - &out_relation->angular_velocity); - } - } else { - if (tmp_flags & XRT_SPACE_RELATION_POSITION_VALID_BIT) { - outf = (enum xrt_space_relation_flags)(outf | XRT_SPACE_RELATION_POSITION_VALID_BIT); - outf = (enum xrt_space_relation_flags)(outf | XRT_SPACE_RELATION_POSITION_TRACKED_BIT); - - outf = (enum xrt_space_relation_flags)(outf | XRT_SPACE_RELATION_LINEAR_VELOCITY_VALID_BIT); - - out_relation->linear_velocity = (in_relation->pose.position - last_relation.pose.position) / dt; - } + out_relation->linear_velocity = (in_relation->pose.position - last_relation.pose.position) / dt; + } - if (tmp_flags & XRT_SPACE_RELATION_ORIENTATION_VALID_BIT) { - outf = (enum xrt_space_relation_flags)(outf | XRT_SPACE_RELATION_ORIENTATION_VALID_BIT); - outf = (enum xrt_space_relation_flags)(outf | XRT_SPACE_RELATION_ORIENTATION_TRACKED_BIT); + if (tmp_flags & XRT_SPACE_RELATION_ORIENTATION_VALID_BIT) { + outf = (enum xrt_space_relation_flags)(outf | XRT_SPACE_RELATION_ORIENTATION_VALID_BIT); + outf = (enum xrt_space_relation_flags)(outf | XRT_SPACE_RELATION_ORIENTATION_TRACKED_BIT); - outf = (enum xrt_space_relation_flags)(outf | XRT_SPACE_RELATION_ANGULAR_VELOCITY_VALID_BIT); + outf = (enum xrt_space_relation_flags)(outf | XRT_SPACE_RELATION_ANGULAR_VELOCITY_VALID_BIT); - math_quat_finite_difference(&last_relation.pose.orientation, &in_relation->pose.orientation, dt, - &out_relation->angular_velocity); - } + math_quat_finite_difference(&last_relation.pose.orientation, &in_relation->pose.orientation, dt, + &out_relation->angular_velocity); } out_relation->pose = in_relation->pose; diff --git a/src/xrt/auxiliary/math/m_relation_history.h b/src/xrt/auxiliary/math/m_relation_history.h index 02556e8ac..2faee0cb0 100644 --- a/src/xrt/auxiliary/math/m_relation_history.h +++ b/src/xrt/auxiliary/math/m_relation_history.h @@ -13,8 +13,6 @@ #include "xrt/xrt_defines.h" -#include "math/m_filter_one_euro.h" - #ifdef __cplusplus extern "C" { #endif @@ -44,19 +42,13 @@ enum m_relation_history_result M_RELATION_HISTORY_RESULT_REVERSE_PREDICTED, //!< The desired timestamp was older than the oldest entry }; -struct m_relation_history_filters -{ - struct m_filter_euro_vec3 position; - struct m_filter_euro_quat orientation; -}; - /*! * Creates an opaque relation_history object. * * @public @memberof m_relation_history */ void -m_relation_history_create(struct m_relation_history **rh, struct m_relation_history_filters *motion_vector_filters); +m_relation_history_create(struct m_relation_history **rh); /*! * Pushes a new pose to the history. @@ -167,7 +159,7 @@ private: public: // clang-format off - RelationHistory(struct m_relation_history_filters *motion_vector_filters) noexcept { m_relation_history_create(&mPtr, motion_vector_filters); } + RelationHistory() noexcept { m_relation_history_create(&mPtr); } ~RelationHistory() { m_relation_history_destroy(&mPtr); } // clang-format on diff --git a/src/xrt/auxiliary/tracking/t_tracker_slam.cpp b/src/xrt/auxiliary/tracking/t_tracker_slam.cpp index 9c8fa5782..0059a7e59 100644 --- a/src/xrt/auxiliary/tracking/t_tracker_slam.cpp +++ b/src/xrt/auxiliary/tracking/t_tracker_slam.cpp @@ -286,14 +286,14 @@ struct TrackerSlam //! Type of prediction to use t_slam_prediction_type pred_type; - u_var_combo pred_combo; //!< UI combo box to select @ref pred_type - RelationHistory slam_rels{nullptr}; //!< A history of relations produced purely from external SLAM tracker data - int dbg_pred_every = 1; //!< Skip X SLAM poses so that you get tracked mostly by the prediction algo - int dbg_pred_counter = 0; //!< SLAM pose counter for prediction debugging - struct os_mutex lock_ff; //!< Lock for gyro_ff and accel_ff. - struct m_ff_vec3_f32 *gyro_ff; //!< Last gyroscope samples - struct m_ff_vec3_f32 *accel_ff; //!< Last accelerometer samples - vector ui_sink; //!< Sink to display frames in UI of each camera + u_var_combo pred_combo; //!< UI combo box to select @ref pred_type + RelationHistory slam_rels{}; //!< A history of relations produced purely from external SLAM tracker data + int dbg_pred_every = 1; //!< Skip X SLAM poses so that you get tracked mostly by the prediction algo + int dbg_pred_counter = 0; //!< SLAM pose counter for prediction debugging + struct os_mutex lock_ff; //!< Lock for gyro_ff and accel_ff. + struct m_ff_vec3_f32 *gyro_ff; //!< Last gyroscope samples + struct m_ff_vec3_f32 *accel_ff; //!< Last accelerometer samples + vector ui_sink; //!< Sink to display frames in UI of each camera //! Used to correct accelerometer measurements when integrating into the prediction. //! @todo Should be automatically computed instead of required to be filled manually through the UI. diff --git a/src/xrt/auxiliary/util/u_tracked_imu_3dof.c b/src/xrt/auxiliary/util/u_tracked_imu_3dof.c index 719a6a6a8..7ef219953 100644 --- a/src/xrt/auxiliary/util/u_tracked_imu_3dof.c +++ b/src/xrt/auxiliary/util/u_tracked_imu_3dof.c @@ -57,7 +57,7 @@ u_tracked_imu_3dof_create(struct xrt_frame_context *xfctx, struct u_tracked_imu_ { struct u_tracked_imu_3dof *dof3 = U_TYPED_CALLOC(struct u_tracked_imu_3dof); - m_relation_history_create(&dof3->rh, NULL); + m_relation_history_create(&dof3->rh); m_imu_3dof_init(&dof3->fusion, M_IMU_3DOF_USE_GRAVITY_DUR_300MS); m_imu_3dof_add_vars(&dof3->fusion, debug_var_root, ""); diff --git a/src/xrt/drivers/realsense/rs_ddev.c b/src/xrt/drivers/realsense/rs_ddev.c index 8e2ee2e09..04284e5af 100644 --- a/src/xrt/drivers/realsense/rs_ddev.c +++ b/src/xrt/drivers/realsense/rs_ddev.c @@ -432,7 +432,7 @@ rs_ddev_create(int device_idx) { struct rs_ddev *rs = U_DEVICE_ALLOCATE(struct rs_ddev, U_DEVICE_ALLOC_TRACKING_NONE, 1, 0); - m_relation_history_create(&rs->relation_hist, NULL); + m_relation_history_create(&rs->relation_hist); rs->enable_mapping = true; rs->enable_pose_jumping = true; diff --git a/src/xrt/drivers/sample/sample_hmd.c b/src/xrt/drivers/sample/sample_hmd.c index ddf91e9c5..744573efc 100644 --- a/src/xrt/drivers/sample/sample_hmd.c +++ b/src/xrt/drivers/sample/sample_hmd.c @@ -195,7 +195,7 @@ sample_hmd_create(void) snprintf(hmd->base.str, XRT_DEVICE_NAME_LEN, "Sample HMD"); snprintf(hmd->base.serial, XRT_DEVICE_NAME_LEN, "Sample HMD S/N"); - m_relation_history_create(&hmd->relation_hist, NULL); + m_relation_history_create(&hmd->relation_hist); // Setup input. hmd->base.name = XRT_DEVICE_GENERIC_HMD; diff --git a/src/xrt/drivers/steamvr_lh/device.cpp b/src/xrt/drivers/steamvr_lh/device.cpp index ceffc3857..7584f8d75 100644 --- a/src/xrt/drivers/steamvr_lh/device.cpp +++ b/src/xrt/drivers/steamvr_lh/device.cpp @@ -197,7 +197,7 @@ Device::~Device() Device::Device(const DeviceBuilder &builder) : xrt_device({}), ctx(builder.ctx), driver(builder.driver) { - m_relation_history_create(&relation_hist, NULL); + m_relation_history_create(&relation_hist); std::strncpy(this->serial, builder.serial, XRT_DEVICE_NAME_LEN - 1); this->serial[XRT_DEVICE_NAME_LEN - 1] = 0; this->tracking_origin = ctx.get(); diff --git a/src/xrt/drivers/survive/survive_driver.c b/src/xrt/drivers/survive/survive_driver.c index 62791e764..4781642fa 100644 --- a/src/xrt/drivers/survive/survive_driver.c +++ b/src/xrt/drivers/survive/survive_driver.c @@ -944,7 +944,7 @@ _create_hmd_device(struct survive_system *sys, const struct SurviveSimpleObject survive->base.tracking_origin = &sys->base; SURVIVE_INFO(survive, "survive HMD present"); - m_relation_history_create(&survive->relation_hist, NULL); + m_relation_history_create(&survive->relation_hist); size_t idx = 0; @@ -1093,7 +1093,7 @@ _create_controller_device(struct survive_system *sys, int outputs = 1; struct survive_device *survive = U_DEVICE_ALLOCATE(struct survive_device, flags, inputs, outputs); survive->ctrl.config = *config; - m_relation_history_create(&survive->relation_hist, NULL); + m_relation_history_create(&survive->relation_hist); sys->controllers[idx] = survive; survive->sys = sys; diff --git a/src/xrt/drivers/vive/vive_controller.c b/src/xrt/drivers/vive/vive_controller.c index 870e580d5..b38560db1 100644 --- a/src/xrt/drivers/vive/vive_controller.c +++ b/src/xrt/drivers/vive/vive_controller.c @@ -1063,7 +1063,7 @@ vive_controller_create(struct os_hid_device *controller_hid, enum watchman_gen w d->watchman_gen = watchman_gen; m_imu_3dof_init(&d->fusion.i3dof, M_IMU_3DOF_USE_GRAVITY_DUR_20MS); - m_relation_history_create(&d->fusion.relation_hist, NULL); + m_relation_history_create(&d->fusion.relation_hist); int ret = os_mutex_init(&d->fusion.mutex); if (ret != 0) { VIVE_ERROR(d, "Failed to init 3dof mutex"); diff --git a/src/xrt/drivers/vive/vive_device.c b/src/xrt/drivers/vive/vive_device.c index d247553fb..e975058c5 100644 --- a/src/xrt/drivers/vive/vive_device.c +++ b/src/xrt/drivers/vive/vive_device.c @@ -1081,7 +1081,7 @@ vive_device_create(struct os_hid_device *mainboard_dev, (enum u_device_alloc_flags)(U_DEVICE_ALLOC_HMD | U_DEVICE_ALLOC_TRACKING_NONE); struct vive_device *d = U_DEVICE_ALLOCATE(struct vive_device, flags, 1, 0); - m_relation_history_create(&d->fusion.relation_hist, NULL); + m_relation_history_create(&d->fusion.relation_hist); size_t idx = 0; d->base.hmd->blend_modes[idx++] = XRT_BLEND_MODE_OPAQUE; diff --git a/src/xrt/drivers/xreal_air/xreal_air_hmd.c b/src/xrt/drivers/xreal_air/xreal_air_hmd.c index 2bd7d0b91..3e50b0829 100644 --- a/src/xrt/drivers/xreal_air/xreal_air_hmd.c +++ b/src/xrt/drivers/xreal_air/xreal_air_hmd.c @@ -1210,7 +1210,7 @@ xreal_air_hmd_create_device(struct os_hid_device *sensor_device, u_distortion_mesh_set_none(&hmd->base); m_imu_3dof_init(&hmd->fusion, M_IMU_3DOF_USE_GRAVITY_DUR_20MS); - m_relation_history_create(&hmd->relation_hist, NULL); + m_relation_history_create(&hmd->relation_hist); hmd->static_id = 0; hmd->display_on = false; diff --git a/src/xrt/tracking/hand/t_hand_tracking_async.c b/src/xrt/tracking/hand/t_hand_tracking_async.c index 45b389296..0102a7589 100644 --- a/src/xrt/tracking/hand/t_hand_tracking_async.c +++ b/src/xrt/tracking/hand/t_hand_tracking_async.c @@ -333,7 +333,7 @@ t_hand_tracking_async_default_create(struct xrt_frame_context *xfctx, struct t_h hta->provider = sync; for (int i = 0; i < 2; i++) { - m_relation_history_create(&hta->present.relation_hist[i], NULL); + m_relation_history_create(&hta->present.relation_hist[i]); } /*! diff --git a/tests/tests_history_buf.cpp b/tests/tests_history_buf.cpp index 9b018fe60..d39ec39da 100644 --- a/tests/tests_history_buf.cpp +++ b/tests/tests_history_buf.cpp @@ -31,7 +31,7 @@ TEST_CASE("m_relation_history") { m_relation_history *rh = nullptr; - m_relation_history_create(&rh, nullptr); + m_relation_history_create(&rh); SECTION("empty buffer") { xrt_space_relation out_relation = XRT_SPACE_RELATION_ZERO; @@ -125,7 +125,7 @@ TEST_CASE("m_relation_history") TEST_CASE("RelationHistory") { using xrt::auxiliary::math::RelationHistory; - RelationHistory rh{nullptr}; + RelationHistory rh; SECTION("empty buffer") {