Commit 18f97609 authored by Ryan Pavlik's avatar Ryan Pavlik Committed by Jakob Bornecrantz

tracking: Add t_imu: simple IMU fusion for absolute orientation

parent b00f5315
......@@ -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',
......
// 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;
}
// 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
// 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
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment