mirrors/monado
1
0
Fork 0
mirror of https://gitlab.freedesktop.org/monado/monado.git synced 2025-01-01 12:46:12 +00:00
Code Issues Actions Packages Projects Releases Wiki Activity

d/psmv: Port positional tracking to use t_imu_fusion with timestamps

Browse source
This commit is contained in:
Ryan Pavlik 2019-11-11 15:09:04 -06:00 committed by Jakob Bornecrantz
parent 7bb9fad5e6
commit 194938c405
5 changed files with 29 additions and 24 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

8
src/xrt/auxiliary/tracking/t_tracker_psmv.cpp
View file

@ -449,7 +449,7 @@ get_pose(TrackerPSMV &t,
static void
imu_data(TrackerPSMV &t,
time_duration_ns delta_ns,
timepoint_ns timestamp_ns,
struct xrt_tracking_sample *sample)
{
os_thread_helper_lock(&t.oth);
@ -459,7 +459,7 @@ imu_data(TrackerPSMV &t,
os_thread_helper_unlock(&t.oth);
return;
}
t.filter->process_imu_data(delta_ns, sample, NULL);
t.filter->process_imu_data(timestamp_ns, sample, NULL);
os_thread_helper_unlock(&t.oth);
}
@ -497,11 +497,11 @@ break_apart(TrackerPSMV &t)
extern "C" void
t_psmv_push_imu(struct xrt_tracked_psmv *xtmv,
time_duration_ns delta_ns,
timepoint_ns timestamp_ns,
struct xrt_tracking_sample *sample)
{
auto &t = *container_of(xtmv, TrackerPSMV, base);
imu_data(t, delta_ns, sample);
imu_data(t, timestamp_ns, sample);
}
extern "C" void

31
src/xrt/auxiliary/tracking/t_tracker_psmv_fusion.cpp
View file

@ -50,13 +50,13 @@ namespace {
clear_position_tracked_flag() override;
void
process_imu_data(time_duration_ns delta_ns,
process_imu_data(timepoint_ns timestamp_ns,
const struct xrt_tracking_sample *sample,
const struct xrt_vec3
*orientation_variance_optional) override;
void
process_3d_vision_data(
time_duration_ns delta_ns,
timepoint_ns timestamp_ns,
const struct xrt_vec3 *position,
const struct xrt_vec3 *variance_optional,
const struct xrt_vec3 *lever_arm_optional,
@ -78,6 +78,7 @@ namespace {
xrt_fusion::SimpleIMUFusion imu;
timepoint_ns filter_time_ns{0};
bool tracked{false};
TrackingInfo orientation_state;
TrackingInfo position_state;
@ -108,26 +109,30 @@ namespace {
void
PSMVFusion::process_imu_data(
time_duration_ns delta_ns,
timepoint_ns timestamp_ns,
const struct xrt_tracking_sample *sample,
const struct xrt_vec3 *orientation_variance_optional)
{
float dt = time_ns_to_s(delta_ns);
Eigen::Vector3d variance = Eigen::Vector3d::Constant(0.01);
if (orientation_variance_optional) {
variance = map_vec3(*orientation_variance_optional)
.cast<double>();
}
imu.handleAccel(map_vec3(sample->accel_m_s2).cast<double>(),
dt);
timestamp_ns);
imu.handleGyro(map_vec3(sample->gyro_rad_secs).cast<double>(),
dt);
timestamp_ns);
imu.postCorrect();
//! @todo use better measurements instead of the above "simple
//! fusion"
flexkalman::predict(filter_state, process_model, dt);
if (filter_time_ns != 0 && filter_time_ns != timestamp_ns) {
float dt = time_ns_to_s(timestamp_ns - filter_time_ns);
assert(dt > 0);
flexkalman::predict(filter_state, process_model, dt);
}
filter_time_ns = timestamp_ns;
auto meas = flexkalman::AbsoluteOrientationMeasurement{
// Must rotate by 180 to align
Eigen::Quaterniond(
@ -156,7 +161,7 @@ namespace {
void
PSMVFusion::process_3d_vision_data(
time_duration_ns delta_ns,
timepoint_ns timestamp_ns,
const struct xrt_vec3 *position,
const struct xrt_vec3 *variance_optional,
const struct xrt_vec3 *lever_arm_optional,
@ -209,12 +214,12 @@ namespace {
// Clear to sane values
U_ZERO(out_relation);
out_relation->pose.orientation.w = 1;
if (!tracked) {
if (!tracked || filter_time_ns == 0) {
return;
}
auto predicted_state = flexkalman::getPrediction(
filter_state, process_model,
/*! @todo compute dt here */ 0.024);
float dt = time_ns_to_s(when_ns - filter_time_ns);
auto predicted_state =
flexkalman::getPrediction(filter_state, process_model, dt);
map_vec3(out_relation->pose.position) =
predicted_state.position().cast<float>();
@ -253,4 +258,4 @@ PSMVFusionInterface::create()
auto ret = std::make_unique<PSMVFusion>();
return ret;
}
} // namespace xrt_fusion
} // namespace xrt_fusion

6
src/xrt/auxiliary/tracking/t_tracker_psmv_fusion.h
View file

@ -33,11 +33,11 @@ public:
virtual void
process_imu_data(
time_duration_ns delta_ns,
timepoint_ns timestamp_ns,
const struct xrt_tracking_sample *sample,
const struct xrt_vec3 *orientation_variance_optional) = 0;
virtual void
process_3d_vision_data(time_duration_ns delta_ns,
process_3d_vision_data(timepoint_ns timestamp_ns,
const struct xrt_vec3 *position,
const struct xrt_vec3 *variance_optional,
const struct xrt_vec3 *lever_arm_optional,
@ -47,4 +47,4 @@ public:
get_prediction(timepoint_ns when_ns,
struct xrt_space_relation *out_relation) = 0;
};
} // namespace xrt_fusion
} // namespace xrt_fusion

2
src/xrt/drivers/psmv/psmv_driver.c
View file

@ -640,7 +640,7 @@ update_fusion(struct psmv_device *psmv,
sample.accel_m_s2 = psmv->read.accel;
sample.gyro_rad_secs = psmv->read.gyro;
xrt_tracked_psmv_push_imu(psmv->ball, delta_ns, &sample);
xrt_tracked_psmv_push_imu(psmv->ball, timestamp_ns, &sample);
} else {
#if 0

6
src/xrt/include/xrt/xrt_tracking.h
View file

@ -115,7 +115,7 @@ struct xrt_tracked_psmv
* Push a IMU sample into the tracking system.
*/
void (*push_imu)(struct xrt_tracked_psmv *,
time_duration_ns delta_ns,
timepoint_ns timestamp_ns,
struct xrt_tracking_sample *sample);
/*!
@ -192,10 +192,10 @@ xrt_tracked_psmv_get_tracked_pose(struct xrt_tracked_psmv *psmv,
static inline void
xrt_tracked_psmv_push_imu(struct xrt_tracked_psmv *psmv,
time_duration_ns delta_ns,
timepoint_ns timestamp_ns,
struct xrt_tracking_sample *sample)
{
psmv->push_imu(psmv, delta_ns, sample);
psmv->push_imu(psmv, timestamp_ns, sample);
}
static inline void