matrixfurry.com
The open source OpenXR runtime
1// Copyright 2019, Collabora, Ltd.
2// SPDX-License-Identifier: BSL-1.0
3/*!
4 * @file
5 * @brief IMU fusion implementation - for inclusion into the single
6 * kalman-incuding translation unit.
7 * @author Rylie Pavlik <rylie.pavlik@collabora.com>
8 * @ingroup aux_tracking
9 */
10// IWYU pragma: no_include "src/Core/MatrixBase.h"
11
12#include "tracking/t_imu.h"
13#include "tracking/t_imu_fusion.hpp"
14
15#include "math/m_eigen_interop.hpp"
16#include "util/u_misc.h"
17
18#include <memory>
19#include <cassert>
20
21using xrt::auxiliary::tracking::SimpleIMUFusion;
22using namespace xrt::auxiliary::math;
23
24struct imu_fusion
25{
26public:
27 uint64_t time_ns{0};
28
29 SimpleIMUFusion simple_fusion;
30
31
32public:
33 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
34};
35
36/*
37 * API functions
38 */
39struct imu_fusion *
40imu_fusion_create()
41{
42 try {
43 auto fusion = std::make_unique<imu_fusion>();
44 return fusion.release();
45 } catch (...) {
46 return NULL;
47 }
48}
49
50void
51imu_fusion_destroy(struct imu_fusion *fusion)
52{
53 try {
54 delete fusion;
55 } catch (...) {
56 assert(false && "Caught exception on destroy");
57 }
58}
59int
60imu_fusion_incorporate_gyros(struct imu_fusion *fusion,
61 uint64_t timestamp_ns,
62 struct xrt_vec3 const *ang_vel,
63 struct xrt_vec3 const *ang_vel_variance)
64{
65 try {
66 assert(fusion);
67 assert(ang_vel);
68 assert(ang_vel_variance);
69 assert(timestamp_ns != 0);
70
71 fusion->simple_fusion.handleGyro(map_vec3(*ang_vel).cast<double>(), timestamp_ns);
72 return 0;
73 } catch (...) {
74 assert(false && "Caught exception on incorporate gyros");
75 return -1;
76 }
77}
78
79int
80imu_fusion_incorporate_accelerometer(struct imu_fusion *fusion,
81 uint64_t timestamp_ns,
82 struct xrt_vec3 const *accel,
83 struct xrt_vec3 const *accel_variance,
84 struct xrt_vec3 *out_world_accel)
85{
86 try {
87 assert(fusion);
88 assert(accel);
89 assert(accel_variance);
90 assert(timestamp_ns != 0);
91 Eigen::Vector3d accelVec = map_vec3(*accel).cast<double>();
92 fusion->simple_fusion.handleAccel(accelVec, timestamp_ns);
93 if (out_world_accel != NULL) {
94 Eigen::Vector3d worldAccel = fusion->simple_fusion.getCorrectedWorldAccel(accelVec);
95 map_vec3(*out_world_accel) = worldAccel.cast<float>();
96 }
97 return 0;
98 } catch (...) {
99 assert(false && "Caught exception on incorporate accelerometer");
100 return -1;
101 }
102}
103
104int
105imu_fusion_get_prediction(struct imu_fusion const *fusion,
106 uint64_t timestamp_ns,
107 struct xrt_quat *out_quat,
108 struct xrt_vec3 *out_ang_vel)
109{
110 try {
111 assert(fusion);
112 assert(out_quat);
113 assert(out_ang_vel);
114 assert(timestamp_ns != 0);
115 if (!fusion->simple_fusion.valid()) {
116 return -2;
117 }
118
119 map_vec3(*out_ang_vel) = fusion->simple_fusion.getAngVel().cast<float>();
120
121 if (timestamp_ns == fusion->time_ns) {
122 // No need to predict here.
123 map_quat(*out_quat) = fusion->simple_fusion.getQuat().cast<float>();
124 return 0;
125 }
126 Eigen::Quaterniond predicted_quat = fusion->simple_fusion.getPredictedQuat(timestamp_ns);
127 map_quat(*out_quat) = predicted_quat.cast<float>();
128 return 0;
129
130 } catch (...) {
131 assert(false && "Caught exception on getting prediction");
132 return -1;
133 }
134}
135
136int
137imu_fusion_get_prediction_rotation_vec(struct imu_fusion const *fusion,
138 uint64_t timestamp_ns,
139 struct xrt_vec3 *out_rotation_vec)
140{
141 try {
142 assert(fusion);
143 assert(out_rotation_vec);
144 assert(timestamp_ns != 0);
145
146 if (!fusion->simple_fusion.valid()) {
147 return -2;
148 }
149 if (timestamp_ns == fusion->time_ns) {
150 // No need to predict here.
151 map_vec3(*out_rotation_vec) = fusion->simple_fusion.getRotationVec().cast<float>();
152 } else {
153 Eigen::Quaterniond predicted_quat = fusion->simple_fusion.getPredictedQuat(timestamp_ns);
154 map_vec3(*out_rotation_vec) = flexkalman::util::quat_ln(predicted_quat).cast<float>();
155 }
156 return 0;
157 } catch (...) {
158 assert(false && "Caught exception on getting prediction");
159 return -1;
160 }
161}
162
163int
164imu_fusion_incorporate_gyros_and_accelerometer(struct imu_fusion *fusion,
165 uint64_t timestamp_ns,
166 struct xrt_vec3 const *ang_vel,
167 struct xrt_vec3 const *ang_vel_variance,
168 struct xrt_vec3 const *accel,
169 struct xrt_vec3 const *accel_variance,
170 struct xrt_vec3 *out_world_accel)
171{
172 try {
173 assert(fusion);
174 assert(ang_vel);
175 assert(ang_vel_variance);
176 assert(accel);
177 assert(accel_variance);
178 assert(timestamp_ns != 0);
179
180 Eigen::Vector3d accelVec = map_vec3(*accel).cast<double>();
181 Eigen::Vector3d angVelVec = map_vec3(*ang_vel).cast<double>();
182 fusion->simple_fusion.handleAccel(accelVec, timestamp_ns);
183 fusion->simple_fusion.handleGyro(angVelVec, timestamp_ns);
184 fusion->simple_fusion.postCorrect();
185 if (out_world_accel != NULL) {
186 Eigen::Vector3d worldAccel = fusion->simple_fusion.getCorrectedWorldAccel(accelVec);
187 map_vec3(*out_world_accel) = worldAccel.cast<float>();
188 }
189 return 0;
190 } catch (...) {
191 assert(false && "Caught exception on incorporate gyros and accelerometer");
192 return -1;
193 }
194}