t_imu.cpp 4.68 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
 * @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
{
19
	uint64_t time_ns{0};
20 21 22 23

	xrt_fusion::SimpleIMUFusion simple_fusion;
};

24 25 26 27 28 29 30 31 32 33
static float
imu_fusion_get_dt(struct imu_fusion &fusion, uint64_t timestamp_ns)
{
	float dt = 0;
	if (fusion.time_ns != 0) {
		dt = (timestamp_ns - fusion.time_ns) * 1.e-9;
	}
	fusion.time_ns = timestamp_ns;
	return dt;
}
34 35 36 37 38

/*
 * API functions
 */
struct imu_fusion *
39
imu_fusion_create()
40
{
41 42 43 44 45 46
	try {
		auto fusion = std::make_unique<imu_fusion>();
		return fusion.release();
	} catch (...) {
		return NULL;
	}
47 48 49
}

void
50 51 52 53 54 55 56
imu_fusion_destroy(struct imu_fusion *fusion)
{
	try {
		delete fusion;
	} catch (...) {
		assert(false && "Caught exception on destroy");
	}
57 58 59
}
int
imu_fusion_incorporate_gyros(struct imu_fusion *fusion,
60
                             uint64_t timestamp_ns,
61
                             struct xrt_vec3 const *ang_vel,
62 63 64 65 66 67 68
                             struct xrt_vec3 const *variance)
{
	try {
		assert(fusion);
		assert(ang_vel);
		assert(variance);

69
		float dt = imu_fusion_get_dt(*fusion, timestamp_ns);
70 71 72 73 74 75 76
		fusion->simple_fusion.handleGyro(
		    map_vec3(*ang_vel).cast<double>(), dt);
		return 0;
	} catch (...) {
		assert(false && "Caught exception on incorporate gyros");
		return -1;
	}
77 78 79 80
}

int
imu_fusion_incorporate_accelerometer(struct imu_fusion *fusion,
81
                                     uint64_t timestamp_ns,
82
                                     struct xrt_vec3 const *accel,
83 84 85 86 87 88 89
                                     struct xrt_vec3 const *variance)
{
	try {
		assert(fusion);
		assert(accel);
		assert(variance);

90
		float dt = imu_fusion_get_dt(*fusion, timestamp_ns);
91 92 93 94 95 96 97 98
		fusion->simple_fusion.handleAccel(
		    map_vec3(*accel).cast<double>(), dt);
		return 0;
	} catch (...) {
		assert(false &&
		       "Caught exception on incorporate accelerometer");
		return -1;
	}
99 100 101 102
}

int
imu_fusion_get_prediction(struct imu_fusion const *fusion,
103
                          uint64_t timestamp_ns,
104
                          struct xrt_quat *out_quat,
105 106 107 108 109 110 111 112 113 114 115 116
                          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>();
117 118

		if (timestamp_ns == fusion->time_ns) {
119 120 121 122 123
			// No need to predict here.
			map_quat(*out_quat) =
			    fusion->simple_fusion.getQuat().cast<float>();
			return 0;
		}
124
		float dt = (timestamp_ns - fusion->time_ns) * 1.e-9;
125 126 127
		Eigen::Quaterniond predicted_quat =
		    fusion->simple_fusion.getPredictedQuat(dt);
		map_quat(*out_quat) = predicted_quat.cast<float>();
128
		return 0;
129 130 131 132

	} catch (...) {
		assert(false && "Caught exception on getting prediction");
		return -1;
133 134 135 136 137
	}
}

int
imu_fusion_get_prediction_rotation_vec(struct imu_fusion const *fusion,
138
                                       uint64_t timestamp_ns,
139 140 141 142 143 144 145 146 147
                                       struct xrt_vec3 *out_rotation_vec)
{
	try {
		assert(fusion);
		assert(out_rotation_vec);

		if (!fusion->simple_fusion.valid()) {
			return -2;
		}
148
		if (timestamp_ns == fusion->time_ns) {
149 150 151 152 153
			// No need to predict here.
			map_vec3(*out_rotation_vec) =
			    fusion->simple_fusion.getRotationVec()
			        .cast<float>();
		} else {
154
			float dt = (timestamp_ns - fusion->time_ns) * 1.e-9;
155 156 157 158 159 160 161 162 163 164
			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;
165 166 167 168 169 170
	}
}

int
imu_fusion_incorporate_gyros_and_accelerometer(
    struct imu_fusion *fusion,
171
    uint64_t timestamp_ns,
172 173 174
    struct xrt_vec3 const *ang_vel,
    struct xrt_vec3 const *ang_vel_variance,
    struct xrt_vec3 const *accel,
175 176 177 178 179 180 181 182 183
    struct xrt_vec3 const *accel_variance)
{
	try {
		assert(fusion);
		assert(ang_vel);
		assert(ang_vel_variance);
		assert(accel);
		assert(accel_variance);

184
		float dt = imu_fusion_get_dt(*fusion, timestamp_ns);
185 186 187 188 189 190 191 192 193 194 195 196
		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;
	}
197
}