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

t/psmv: Use new imu fusion in psmv positional tracking.

Browse source
This commit is contained in:
Ryan Pavlik 2019-10-10 11:22:14 -05:00 committed by Jakob Bornecrantz
parent 18f9760986
commit 430e27d084
1 changed files with 8 additions and 5 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

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

@ -13,6 +13,7 @@
#include "tracking/t_tracking.h"
#include "tracking/t_calibration_opencv.h"
#include "tracking/t_fusion.h"
#include "tracking/t_imu_fusion.h"
#include "util/u_var.h"
#include "util/u_misc.h"
@ -97,6 +98,8 @@ struct TrackerPSMV
State filter_state;
ProcessModel process_model;
xrt_fusion::SimpleIMUFusion imu;
xrt_vec3 tracked_object_position;
Eigen::Isometry3f room_transform;
};
@ -455,14 +458,14 @@ imu_data(TrackerPSMV &t,
}
float dt = time_ns_to_s(delta_ns);
// Super simple fusion.
math_quat_integrate_velocity(&t.fusion.rot, &sample->gyro_rad_secs, dt,
&t.fusion.rot);
t.imu.handleAccel(map_vec3(sample->accel_m_s2).cast<double>(), dt);
t.imu.handleGyro(map_vec3(sample->gyro_rad_secs).cast<double>(), dt);
t.imu.postCorrect();
//! @todo use better measurements instead of the above "simple fusion"
flexkalman::predict(t.filter_state, t.process_model, dt);
Eigen::Quaterniond quat = Eigen::Quaternionf(map_quat(t.fusion.rot)).cast<double>();
auto meas = flexkalman::AbsoluteOrientationMeasurement{
quat, Eigen::Vector3d::Constant(0.01)};
t.imu.getQuat(), Eigen::Vector3d::Constant(0.01)};
if (!flexkalman::correctUnscented(t.filter_state, meas)) {
fprintf(stderr,
"Got non-finite something when filtering IMU!\n");