tracking: Add t_imu: simple IMU fusion for absolute orientation

src/xrt/auxiliary/CMakeLists.txt

@@ -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

src/xrt/auxiliary/meson.build

@@ -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',

src/xrt/auxiliary/tracking/t_imu.cpp

@@ -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;
+}

src/xrt/auxiliary/tracking/t_imu.h

@@ -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

src/xrt/auxiliary/tracking/t_imu_fusion.h

@@ -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