mirror of
https://gitlab.freedesktop.org/monado/monado.git
synced 2025-03-03 21:26:36 +00:00
tracking: Add t_imu: simple IMU fusion for absolute orientation
This commit is contained in:
parent
b00f5315c2
commit
18f9760986
src/xrt/auxiliary
|
@ -33,6 +33,9 @@ set(TRACKING_SOURCE_FILES
|
|||
tracking/t_file.cpp
|
||||
tracking/t_fusion.h
|
||||
tracking/t_hsv_filter.c
|
||||
tracking/t_imu_fusion.h
|
||||
tracking/t_imu.cpp
|
||||
tracking/t_imu.h
|
||||
tracking/t_tracker_psmv.cpp
|
||||
tracking/t_tracker_psvr.cpp
|
||||
tracking/t_tracking.h
|
||||
|
|
|
@ -102,6 +102,9 @@ tracking_srcs = [
|
|||
'tracking/t_file.cpp',
|
||||
'tracking/t_fusion.h',
|
||||
'tracking/t_hsv_filter.c',
|
||||
'tracking/t_imu_fusion.h',
|
||||
'tracking/t_imu.cpp',
|
||||
'tracking/t_imu.h',
|
||||
'tracking/t_tracker_psmv.cpp',
|
||||
'tracking/t_tracker_psvr.cpp',
|
||||
'tracking/t_tracking.h',
|
||||
|
|
160
src/xrt/auxiliary/tracking/t_imu.cpp
Normal file
160
src/xrt/auxiliary/tracking/t_imu.cpp
Normal file
|
@ -0,0 +1,160 @@
|
|||
// Copyright 2019, Collabora, Ltd.
|
||||
// SPDX-License-Identifier: BSL-1.0
|
||||
/*!
|
||||
* @file
|
||||
* @brief IMU fusion.
|
||||
* @author Ryan Pavlik <ryan.pavlik@collabora.com>
|
||||
* @ingroup aux_tracking
|
||||
*/
|
||||
|
||||
#include "t_imu.h"
|
||||
#include "t_imu_fusion.h"
|
||||
#include "math/m_eigen_interop.h"
|
||||
|
||||
#include <memory>
|
||||
|
||||
struct imu_fusion
|
||||
{
|
||||
uint64_t time_ns{};
|
||||
|
||||
xrt_fusion::SimpleIMUFusion simple_fusion;
|
||||
};
|
||||
|
||||
|
||||
/*
|
||||
* API functions
|
||||
*/
|
||||
struct imu_fusion *
|
||||
imu_fusion_create() try
|
||||
{
|
||||
auto fusion = std::make_unique<imu_fusion>();
|
||||
return fusion.release();
|
||||
} catch (...) {
|
||||
return NULL;
|
||||
}
|
||||
|
||||
void
|
||||
imu_fusion_destroy(struct imu_fusion *fusion) try {
|
||||
delete fusion;
|
||||
} catch (...) {
|
||||
assert(false && "Caught exception on destroy");
|
||||
}
|
||||
|
||||
int
|
||||
imu_fusion_incorporate_gyros(struct imu_fusion *fusion,
|
||||
float dt,
|
||||
struct xrt_vec3 const *ang_vel,
|
||||
struct xrt_vec3 const *variance) try {
|
||||
assert(fusion);
|
||||
assert(ang_vel);
|
||||
assert(variance);
|
||||
|
||||
fusion->simple_fusion.handleGyro(map_vec3(*ang_vel).cast<double>(), dt);
|
||||
return 0;
|
||||
} catch (...) {
|
||||
assert(false && "Caught exception on incorporate gyros");
|
||||
return -1;
|
||||
}
|
||||
|
||||
int
|
||||
imu_fusion_incorporate_accelerometer(struct imu_fusion *fusion,
|
||||
float dt,
|
||||
struct xrt_vec3 const *accel,
|
||||
float scale,
|
||||
struct xrt_vec3 const *reference,
|
||||
struct xrt_vec3 const *variance) try {
|
||||
assert(fusion);
|
||||
assert(accel);
|
||||
assert(variance);
|
||||
|
||||
fusion->simple_fusion.handleAccel(map_vec3(*accel).cast<double>(), dt);
|
||||
return 0;
|
||||
} catch (...) {
|
||||
assert(false && "Caught exception on incorporate accelerometer");
|
||||
return -1;
|
||||
}
|
||||
|
||||
int
|
||||
imu_fusion_get_prediction(struct imu_fusion const *fusion,
|
||||
float dt,
|
||||
struct xrt_quat *out_quat,
|
||||
struct xrt_vec3 *out_ang_vel) try {
|
||||
assert(fusion);
|
||||
assert(out_quat);
|
||||
assert(out_ang_vel);
|
||||
if (!fusion->simple_fusion.valid()) {
|
||||
return -2;
|
||||
}
|
||||
|
||||
map_vec3(*out_ang_vel) =
|
||||
fusion->simple_fusion.getAngVel().cast<float>();
|
||||
if (dt == 0) {
|
||||
// No need to predict here.
|
||||
map_quat(*out_quat) =
|
||||
fusion->simple_fusion.getQuat().cast<float>();
|
||||
return 0;
|
||||
}
|
||||
Eigen::Quaterniond predicted_quat =
|
||||
fusion->simple_fusion.getPredictedQuat(dt);
|
||||
map_quat(*out_quat) = predicted_quat.cast<float>();
|
||||
return 0;
|
||||
|
||||
} catch (...) {
|
||||
assert(false && "Caught exception on getting prediction");
|
||||
return -1;
|
||||
}
|
||||
|
||||
int
|
||||
imu_fusion_get_prediction_rotation_vec(struct imu_fusion const *fusion,
|
||||
float dt,
|
||||
struct xrt_vec3 *out_rotation_vec) try {
|
||||
assert(fusion);
|
||||
assert(out_rotation_vec);
|
||||
|
||||
if (!fusion->simple_fusion.valid()) {
|
||||
return -2;
|
||||
}
|
||||
if (dt == 0) {
|
||||
// No need to predict here.
|
||||
map_vec3(*out_rotation_vec) =
|
||||
fusion->simple_fusion.getRotationVec().cast<float>();
|
||||
} else {
|
||||
Eigen::Quaterniond predicted_quat =
|
||||
fusion->simple_fusion.getPredictedQuat(dt);
|
||||
map_vec3(*out_rotation_vec) =
|
||||
flexkalman::util::quat_ln(predicted_quat).cast<float>();
|
||||
}
|
||||
return 0;
|
||||
} catch (...) {
|
||||
assert(false && "Caught exception on getting prediction");
|
||||
return -1;
|
||||
}
|
||||
|
||||
int
|
||||
imu_fusion_incorporate_gyros_and_accelerometer(
|
||||
struct imu_fusion *fusion,
|
||||
float dt,
|
||||
struct xrt_vec3 const *ang_vel,
|
||||
struct xrt_vec3 const *ang_vel_variance,
|
||||
struct xrt_vec3 const *accel,
|
||||
float accel_scale,
|
||||
struct xrt_vec3 const *accel_reference,
|
||||
struct xrt_vec3 const *accel_variance) try {
|
||||
assert(fusion);
|
||||
assert(ang_vel);
|
||||
assert(ang_vel_variance);
|
||||
assert(accel);
|
||||
assert(accel_reference);
|
||||
assert(accel_variance);
|
||||
|
||||
Eigen::Vector3d accelVec = map_vec3(*accel).cast<double>();
|
||||
Eigen::Vector3d angVelVec = map_vec3(*ang_vel).cast<double>();
|
||||
fusion->simple_fusion.handleAccel(accelVec, dt);
|
||||
fusion->simple_fusion.handleGyro(angVelVec, dt);
|
||||
fusion->simple_fusion.postCorrect();
|
||||
return 0;
|
||||
} catch (...) {
|
||||
assert(false &&
|
||||
"Caught exception on incorporate gyros and accelerometer");
|
||||
return -1;
|
||||
}
|
132
src/xrt/auxiliary/tracking/t_imu.h
Normal file
132
src/xrt/auxiliary/tracking/t_imu.h
Normal file
|
@ -0,0 +1,132 @@
|
|||
// Copyright 2019, Collabora, Ltd.
|
||||
// SPDX-License-Identifier: BSL-1.0
|
||||
/*!
|
||||
* @file
|
||||
* @brief C interface to basic IMU fusion.
|
||||
* @author Ryan Pavlik <ryan.pavlik@collabora.com>
|
||||
*
|
||||
* @ingroup aux_tracking
|
||||
*/
|
||||
|
||||
#include "math/m_api.h"
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
|
||||
struct imu_fusion;
|
||||
/*!
|
||||
* Create a struct imu_fusion.
|
||||
*
|
||||
* @public @memberof imu_fusion
|
||||
* @ingroup aux_tracking
|
||||
*/
|
||||
struct imu_fusion *
|
||||
imu_fusion_create();
|
||||
|
||||
|
||||
/*!
|
||||
* Destroy a struct imu_fusion.
|
||||
*
|
||||
* Should not be called simultaneously with any other imu_fusion function.
|
||||
*
|
||||
* @public @memberof imu_fusion
|
||||
* @ingroup aux_tracking
|
||||
*/
|
||||
void
|
||||
imu_fusion_destroy(struct imu_fusion *fusion);
|
||||
|
||||
/*!
|
||||
* Predict and correct fusion with a gyroscope reading.
|
||||
*
|
||||
* dt should not be zero: If you're receiving accel and gyro data at the same
|
||||
* time, call imu_fusion_incorporate_gyros_and_accelerometer() instead.
|
||||
*
|
||||
* Should not be called simultaneously with any other imu_fusion function.
|
||||
*
|
||||
* Non-zero return means error.
|
||||
*
|
||||
* @public @memberof imu_fusion
|
||||
* @ingroup aux_tracking
|
||||
*/
|
||||
int
|
||||
imu_fusion_incorporate_gyros(struct imu_fusion *fusion,
|
||||
float dt,
|
||||
struct xrt_vec3 const *ang_vel,
|
||||
struct xrt_vec3 const *variance);
|
||||
|
||||
/*!
|
||||
* Predict and correct fusion with an accelerometer reading.
|
||||
*
|
||||
* dt should not be zero: If you're receiving accel and gyro data at the same
|
||||
* time, call imu_fusion_incorporate_gyros_and_accelerometer() instead.
|
||||
*
|
||||
* Should not be called simultaneously with any other imu_fusion function.
|
||||
*
|
||||
* Non-zero return means error.
|
||||
*
|
||||
* @public @memberof imu_fusion
|
||||
* @ingroup aux_tracking
|
||||
*/
|
||||
int
|
||||
imu_fusion_incorporate_accelerometer(struct imu_fusion *fusion,
|
||||
float dt,
|
||||
struct xrt_vec3 const *accel,
|
||||
float scale,
|
||||
struct xrt_vec3 const *reference,
|
||||
struct xrt_vec3 const *variance);
|
||||
|
||||
/*!
|
||||
* Predict and correct fusion with a simultaneous accelerometer and gyroscope
|
||||
* reading.
|
||||
*
|
||||
* Should not be called simultaneously with any other imu_fusion function.
|
||||
*
|
||||
* Non-zero return means error.
|
||||
*
|
||||
* @public @memberof imu_fusion
|
||||
* @ingroup aux_tracking
|
||||
*/
|
||||
int
|
||||
imu_fusion_incorporate_gyros_and_accelerometer(
|
||||
struct imu_fusion *fusion,
|
||||
float dt,
|
||||
struct xrt_vec3 const *ang_vel,
|
||||
struct xrt_vec3 const *ang_vel_variance,
|
||||
struct xrt_vec3 const *accel,
|
||||
float accel_scale,
|
||||
struct xrt_vec3 const *accel_reference,
|
||||
struct xrt_vec3 const *accel_variance);
|
||||
|
||||
/*!
|
||||
* Get the predicted state. Does not advance the internal state clock.
|
||||
*
|
||||
* Non-zero return means error.
|
||||
*
|
||||
* @public @memberof imu_fusion
|
||||
* @ingroup aux_tracking
|
||||
*/
|
||||
int
|
||||
imu_fusion_get_prediction(struct imu_fusion const *fusion,
|
||||
float dt,
|
||||
struct xrt_quat *out_quat,
|
||||
struct xrt_vec3 *out_ang_vel);
|
||||
|
||||
|
||||
/*!
|
||||
* Get the predicted state as a rotation vector. Does not advance the internal
|
||||
* state clock.
|
||||
*
|
||||
* Non-zero return means error.
|
||||
*
|
||||
* @public @memberof imu_fusion
|
||||
* @ingroup aux_tracking
|
||||
*/
|
||||
int
|
||||
imu_fusion_get_prediction_rotation_vec(struct imu_fusion const *fusion,
|
||||
float dt,
|
||||
struct xrt_vec3 *out_rotation_vec);
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
142
src/xrt/auxiliary/tracking/t_imu_fusion.h
Normal file
142
src/xrt/auxiliary/tracking/t_imu_fusion.h
Normal file
|
@ -0,0 +1,142 @@
|
|||
// Copyright 2019, Collabora, Ltd.
|
||||
// SPDX-License-Identifier: BSL-1.0
|
||||
/*!
|
||||
* @file
|
||||
* @brief C++ sensor fusion/filtering code that uses flexkalman
|
||||
* @author Ryan Pavlik <ryan.pavlik@collabora.com>
|
||||
*
|
||||
* @ingroup aux_tracking
|
||||
*/
|
||||
#pragma once
|
||||
|
||||
|
||||
#ifndef __cplusplus
|
||||
#error "This header is C++-only."
|
||||
#endif
|
||||
|
||||
#include <Eigen/Core>
|
||||
#include <Eigen/Geometry>
|
||||
|
||||
#include "flexkalman/EigenQuatExponentialMap.h"
|
||||
|
||||
namespace xrt_fusion {
|
||||
class SimpleIMUFusion
|
||||
{
|
||||
public:
|
||||
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
|
||||
/*!
|
||||
* @param gravity_rate Value in [0, 1] indicating how much the
|
||||
* accelerometer should affect the orientation each second.
|
||||
*/
|
||||
explicit SimpleIMUFusion(double gravity_rate = 0.9)
|
||||
: gravity_scale_(gravity_rate)
|
||||
{}
|
||||
|
||||
bool
|
||||
valid() const noexcept
|
||||
{
|
||||
return started_;
|
||||
}
|
||||
|
||||
Eigen::Quaterniond
|
||||
getQuat() const
|
||||
{
|
||||
return quat_;
|
||||
}
|
||||
|
||||
Eigen::Quaterniond
|
||||
getPredictedQuat(float dt) const
|
||||
{
|
||||
return quat_ * flexkalman::util::quat_exp(angVel_ * dt * 0.5);
|
||||
}
|
||||
|
||||
Eigen::Vector3d
|
||||
getRotationVec() const
|
||||
{
|
||||
return flexkalman::util::quat_ln(quat_);
|
||||
}
|
||||
|
||||
//! in world space
|
||||
Eigen::Vector3d const &
|
||||
getAngVel() const
|
||||
{
|
||||
return angVel_;
|
||||
}
|
||||
|
||||
bool
|
||||
handleGyro(Eigen::Vector3d const &gyro, float dt)
|
||||
{
|
||||
if (!started_) {
|
||||
return false;
|
||||
}
|
||||
Eigen::Vector3d incRot = gyro * dt;
|
||||
// Crude handling of "approximately zero"
|
||||
if (incRot.squaredNorm() < 1.e-8) {
|
||||
return false;
|
||||
}
|
||||
|
||||
angVel_ = gyro;
|
||||
|
||||
// Update orientation
|
||||
quat_ = quat_ * flexkalman::util::quat_exp(incRot * 0.5);
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
bool
|
||||
handleAccel(Eigen::Vector3d const &accel, float /* dt */)
|
||||
{
|
||||
auto diff = std::abs(accel.norm() - 9.81);
|
||||
if (!started_) {
|
||||
if (diff > 1.) {
|
||||
// We're moving, don't start it now.
|
||||
return false;
|
||||
}
|
||||
|
||||
// Initially, just set it to totally trust gravity.
|
||||
started_ = true;
|
||||
quat_ = Eigen::Quaterniond::FromTwoVectors(
|
||||
Eigen::Vector3d::UnitY(), accel.normalized());
|
||||
return true;
|
||||
}
|
||||
auto scale = 1. - diff;
|
||||
if (scale <= 0) {
|
||||
// Too far from gravity to be useful/trusted.
|
||||
return false;
|
||||
}
|
||||
// This should match the global gravity vector if the rotation
|
||||
// is right.
|
||||
Eigen::Vector3d measuredGravityDirection =
|
||||
(quat_ * accel).normalized();
|
||||
auto incremental = Eigen::Quaterniond::FromTwoVectors(
|
||||
measuredGravityDirection, Eigen::Vector3d::UnitY());
|
||||
|
||||
double alpha = scale * gravity_scale_;
|
||||
Eigen::Quaterniond scaledIncrementalQuat =
|
||||
Eigen::Quaterniond::Identity().slerp(alpha, incremental);
|
||||
|
||||
// Update orientation
|
||||
quat_ = scaledIncrementalQuat * quat_;
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
Eigen::Matrix3d
|
||||
getRotationMatrix() const
|
||||
{
|
||||
return quat_.toRotationMatrix();
|
||||
}
|
||||
|
||||
void
|
||||
postCorrect()
|
||||
{
|
||||
quat_.normalize();
|
||||
}
|
||||
|
||||
private:
|
||||
Eigen::Vector3d angVel_{Eigen::Vector3d::Zero()};
|
||||
Eigen::Quaterniond quat_{Eigen::Quaterniond::Identity()};
|
||||
double gravity_scale_;
|
||||
bool started_{false};
|
||||
};
|
||||
} // namespace xrt_fusion
|
Loading…
Reference in a new issue