t_imu.cpp 4.08 KB
Newer Older
1 2 3 4
// Copyright 2019, Collabora, Ltd.
// SPDX-License-Identifier: BSL-1.0
/*!
 * @file
5 6
 * @brief  IMU fusion implementation - for inclusion into the single
 * kalman-incuding translation unit.
7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28
 * @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 *
29
imu_fusion_create()
30
{
31 32 33 34 35 36
	try {
		auto fusion = std::make_unique<imu_fusion>();
		return fusion.release();
	} catch (...) {
		return NULL;
	}
37 38 39
}

void
40 41 42 43 44 45 46
imu_fusion_destroy(struct imu_fusion *fusion)
{
	try {
		delete fusion;
	} catch (...) {
		assert(false && "Caught exception on destroy");
	}
47 48 49 50 51 52
}

int
imu_fusion_incorporate_gyros(struct imu_fusion *fusion,
                             float dt,
                             struct xrt_vec3 const *ang_vel,
53 54 55 56 57 58 59 60 61 62 63 64 65 66
                             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;
	}
67 68 69 70 71 72
}

int
imu_fusion_incorporate_accelerometer(struct imu_fusion *fusion,
                                     float dt,
                                     struct xrt_vec3 const *accel,
73 74 75 76 77 78 79 80 81 82 83 84 85 86 87
                                     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;
	}
88 89 90 91 92 93
}

int
imu_fusion_get_prediction(struct imu_fusion const *fusion,
                          float dt,
                          struct xrt_quat *out_quat,
94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114
                          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>();
115
		return 0;
116 117 118 119

	} catch (...) {
		assert(false && "Caught exception on getting prediction");
		return -1;
120 121 122 123 124 125
	}
}

int
imu_fusion_get_prediction_rotation_vec(struct imu_fusion const *fusion,
                                       float dt,
126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149 150
                                       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;
151 152 153 154 155 156 157 158 159 160
	}
}

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,
161 162 163 164 165 166 167 168 169 170 171 172 173 174 175 176 177 178 179 180 181
    struct xrt_vec3 const *accel_variance)
{
	try {
		assert(fusion);
		assert(ang_vel);
		assert(ang_vel_variance);
		assert(accel);
		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;
	}
182
}