matrixfurry.com
The open source OpenXR runtime
1// Copyright 2019, Collabora, Ltd.
2// SPDX-License-Identifier: BSL-1.0
3/*!
4 * @file
5 * @brief PS Move tracker code that is expensive to compile.
6 *
7 * Typically built as a part of t_kalman.cpp to reduce incremental build times.
8 *
9 * @author Rylie Pavlik <rylie.pavlik@collabora.com>
10 * @author Pete Black <pblack@collabora.com>
11 * @author Jakob Bornecrantz <jakob@collabora.com>
12 * @ingroup aux_tracking
13 */
14
15#include "tracking/t_fusion.hpp"
16#include "tracking/t_imu_fusion.hpp"
17#include "tracking/t_tracker_psmv_fusion.hpp"
18
19#include "math/m_api.h"
20#include "math/m_eigen_interop.hpp"
21
22#include "util/u_misc.h"
23
24#include "flexkalman/AbsoluteOrientationMeasurement.h"
25#include "flexkalman/FlexibleKalmanFilter.h"
26#include "flexkalman/FlexibleUnscentedCorrect.h"
27#include "flexkalman/PoseSeparatelyDampedConstantVelocity.h"
28#include "flexkalman/PoseState.h"
29
30
31namespace xrt::auxiliary::tracking {
32
33using namespace xrt::auxiliary::math;
34
35//! Anonymous namespace to hide implementation names
36namespace {
37
38 using State = flexkalman::pose_externalized_rotation::State;
39 using ProcessModel = flexkalman::PoseSeparatelyDampedConstantVelocityProcessModel<State>;
40
41 struct TrackingInfo
42 {
43 bool valid{false};
44 bool tracked{false};
45 };
46 class PSMVFusion : public PSMVFusionInterface
47 {
48 public:
49 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
50
51 void
52 clear_position_tracked_flag() override;
53
54 void
55 process_imu_data(timepoint_ns timestamp_ns,
56 const struct xrt_tracking_sample *sample,
57 const struct xrt_vec3 *orientation_variance_optional) override;
58 void
59 process_3d_vision_data(timepoint_ns timestamp_ns,
60 const struct xrt_vec3 *position,
61 const struct xrt_vec3 *variance_optional,
62 const struct xrt_vec3 *lever_arm_optional,
63 float residual_limit) override;
64
65 void
66 get_prediction(timepoint_ns when_ns, struct xrt_space_relation *out_relation) override;
67
68 private:
69 void
70 reset_filter();
71 void
72 reset_filter_and_imu();
73
74 State filter_state;
75 ProcessModel process_model;
76
77 xrt::auxiliary::tracking::SimpleIMUFusion imu;
78
79 timepoint_ns filter_time_ns{0};
80 bool tracked{false};
81 TrackingInfo orientation_state;
82 TrackingInfo position_state;
83 };
84
85
86
87 void
88 PSMVFusion::clear_position_tracked_flag()
89 {
90 position_state.tracked = false;
91 }
92
93 void
94 PSMVFusion::reset_filter()
95 {
96 filter_state = State{};
97 tracked = false;
98 position_state = TrackingInfo{};
99 }
100 void
101 PSMVFusion::reset_filter_and_imu()
102 {
103 reset_filter();
104 orientation_state = TrackingInfo{};
105 imu = SimpleIMUFusion{};
106 }
107
108 void
109 PSMVFusion::process_imu_data(timepoint_ns timestamp_ns,
110 const struct xrt_tracking_sample *sample,
111 const struct xrt_vec3 *orientation_variance_optional)
112 {
113
114 Eigen::Vector3d variance = Eigen::Vector3d::Constant(0.01);
115 if (orientation_variance_optional) {
116 variance = map_vec3(*orientation_variance_optional).cast<double>();
117 }
118 imu.handleAccel(map_vec3(sample->accel_m_s2).cast<double>(), timestamp_ns);
119 imu.handleGyro(map_vec3(sample->gyro_rad_secs).cast<double>(), timestamp_ns);
120 imu.postCorrect();
121
122 //! @todo use better measurements instead of the preceding "simple
123 //! fusion"
124 if (filter_time_ns != 0 && filter_time_ns != timestamp_ns) {
125 float dt = time_ns_to_s(timestamp_ns - filter_time_ns);
126 assert(dt > 0);
127 flexkalman::predict(filter_state, process_model, dt);
128 }
129 filter_time_ns = timestamp_ns;
130 auto meas = flexkalman::AbsoluteOrientationMeasurement{
131 // Must rotate by 180 to align
132 Eigen::Quaterniond(Eigen::AngleAxisd(EIGEN_PI, Eigen::Vector3d::UnitY())) * imu.getQuat(),
133 variance};
134 if (flexkalman::correctUnscented(filter_state, meas)) {
135 orientation_state.tracked = true;
136 orientation_state.valid = true;
137 } else {
138 U_LOG_E(
139 "Got non-finite something when filtering IMU - "
140 "resetting filter and IMU fusion!");
141 reset_filter_and_imu();
142 }
143 // 7200 deg/sec
144 constexpr double max_rad_per_sec = 20 * double(EIGEN_PI) * 2;
145 if (filter_state.angularVelocity().squaredNorm() > max_rad_per_sec * max_rad_per_sec) {
146 U_LOG_E(
147 "Got excessive angular velocity when filtering "
148 "IMU - resetting filter and IMU fusion!");
149 reset_filter_and_imu();
150 }
151 }
152
153 void
154 PSMVFusion::process_3d_vision_data(timepoint_ns timestamp_ns,
155 const struct xrt_vec3 *position,
156 const struct xrt_vec3 *variance_optional,
157 const struct xrt_vec3 *lever_arm_optional,
158 float residual_limit)
159 {
160 Eigen::Vector3f pos = map_vec3(*position);
161 Eigen::Vector3d variance{1.e-4, 1.e-4, 4.e-4};
162 if (variance_optional) {
163 variance = map_vec3(*variance_optional).cast<double>();
164 }
165 Eigen::Vector3d lever_arm{0, 0.09, 0};
166 if (lever_arm_optional) {
167 lever_arm = map_vec3(*lever_arm_optional).cast<double>();
168 }
169 auto measurement = xrt::auxiliary::tracking::AbsolutePositionLeverArmMeasurement{pos.cast<double>(),
170 lever_arm, variance};
171 double resid = measurement.getResidual(filter_state).norm();
172
173 if (resid > residual_limit) {
174 // Residual arbitrarily "too large"
175 U_LOG_W(
176 "measurement residual is %f, resetting "
177 "filter state",
178 resid);
179 reset_filter();
180 return;
181 }
182 if (flexkalman::correctUnscented(filter_state, measurement)) {
183 tracked = true;
184 position_state.valid = true;
185 position_state.tracked = true;
186 } else {
187 U_LOG_W(
188 "Got non-finite something when filtering "
189 "tracker - resetting filter!");
190 reset_filter();
191 }
192 }
193
194 void
195 PSMVFusion::get_prediction(timepoint_ns when_ns, struct xrt_space_relation *out_relation)
196 {
197 if (out_relation == NULL) {
198 return;
199 }
200 // Clear to identity values
201 U_ZERO(out_relation);
202 out_relation->pose.orientation.w = 1;
203 if (!tracked || filter_time_ns == 0) {
204 return;
205 }
206 float dt = time_ns_to_s(when_ns - filter_time_ns);
207 auto predicted_state = flexkalman::getPrediction(filter_state, process_model, dt);
208
209 map_vec3(out_relation->pose.position) = predicted_state.position().cast<float>();
210 map_quat(out_relation->pose.orientation) = predicted_state.getQuaternion().cast<float>();
211 map_vec3(out_relation->linear_velocity) = predicted_state.velocity().cast<float>();
212 map_vec3(out_relation->angular_velocity) = predicted_state.angularVelocity().cast<float>();
213
214 uint64_t flags = 0;
215 if (position_state.valid) {
216 flags |= XRT_SPACE_RELATION_POSITION_VALID_BIT;
217 flags |= XRT_SPACE_RELATION_LINEAR_VELOCITY_VALID_BIT;
218 if (position_state.tracked) {
219 flags |= XRT_SPACE_RELATION_POSITION_TRACKED_BIT;
220 }
221 }
222 if (orientation_state.valid) {
223 flags |= XRT_SPACE_RELATION_ORIENTATION_VALID_BIT;
224 flags |= XRT_SPACE_RELATION_ANGULAR_VELOCITY_VALID_BIT;
225 if (orientation_state.tracked) {
226 flags |= XRT_SPACE_RELATION_ORIENTATION_TRACKED_BIT;
227 }
228 }
229 out_relation->relation_flags = (xrt_space_relation_flags)flags;
230 }
231} // namespace
232
233
234std::unique_ptr<PSMVFusionInterface>
235PSMVFusionInterface::create()
236{
237 auto ret = std::make_unique<PSMVFusion>();
238 return ret;
239}
240} // namespace xrt::auxiliary::tracking