Monado OpenXR Runtime
Loading...
Searching...
No Matches
imu_preintegration.hpp
Go to the documentation of this file.
1// Copyright 2026, Beyley Cardellio
2// SPDX-License-Identifier: BSL-1.0
3/*!
4 * @file
5 * @brief Generic math helpers for IMU preintegration.
6 * @author Beyley Cardellio <ep1cm1n10n123@gmail.com>
7 * @ingroup tracking
8 */
9
10#pragma once
11
12#include "os/os_time.h"
13
15#include "math/m_quatexpmap.hpp"
17
18#include <Eigen/Core>
19#include <Eigen/Geometry>
20
21#include "ceres/jet.h"
22
23#include "internal_math.hpp"
24
25#include <cstring>
26#include <cmath>
27#include <mutex>
28#include <string>
29#include <algorithm>
30#include <span>
31#include <cassert>
32
33
34namespace xrt::tracking::constellation::optimizer {
35
36using namespace xrt::auxiliary::math;
37
38using Eigen::Map;
39using Eigen::Matrix;
40using Eigen::Matrix3d;
41using Eigen::MatrixBase;
42using Eigen::Quaternion;
43using Eigen::Quaterniond;
44using Eigen::Vector;
45using Eigen::Vector2;
46using Eigen::Vector3;
47using Eigen::Vector3d;
48using Eigen::Vector4;
49
50using ceres::Jet;
51
52// @todo Tune this per device. Right now these are CV1 HMD numbers.
53//! BMI055 gyroscope noise density, 0.014 deg/s/sqrt(Hz) from the datasheet, in rad/s/sqrt(Hz).
54constexpr double kGyroNoiseDensity = 2.44e-4;
55//! BMI055 accelerometer noise density, 150 ug/sqrt(Hz) from the datasheet, in (m/s^2)/sqrt(Hz).
56constexpr double kAccelNoiseDensity = 1.47e-3;
57
58// @todo These need to be computed properly, not guessed.
59constexpr double kAccelBiasRandomWalkSigma = 1e-3; // (m/s^2) / sqrt(s)
60constexpr double kGyroBiasRandomWalkSigma = 1e-4; // (rad/s) / sqrt(s)
61constexpr double kAccelScaleRandomWalkSigma = 1e-5; // 1 / sqrt(s)
62
63// @todo This needs to be computed properly, not hardcoded for CV1.
64constexpr double kAccelBiasAnchorSigma = 0.687; // m/s^2
65constexpr double kGyroBiasAnchorSigma = 0.0175; // rad/s
66constexpr double kAccelScaleAnchorSigma = 0.05; // dimensionless
67
68//! Largest accelerometer bias to allow, in m/s^2. The BMI055's zero-g offset is +-70mg, so this is ~1.4x that.
69constexpr double kMaxAccelBias = 1.0;
70//! Largest gyroscope bias to allow, in rad/s. The BMI055's zero-rate offset is +-1 deg/s, this is ~3 deg/s.
71constexpr double kMaxGyroBias = 0.05;
72
73// Some reasonable defaults. This should be pretty close to 1.0 already in most cases.
74constexpr double kMinAccelScale = 0.8;
75constexpr double kMaxAccelScale = 1.0 / kMinAccelScale;
76
77/*!
78 * Camera samples from the same mosaic closer together than this are treated as one keyframe. Sometimes frame timestamps
79 * are a couple microseconds off due to guesswork/interpolation, this groups them together.
80 *
81 * @todo We should by default group based on sequence_id! However, to make external users more reliable, let's not rely
82 * on that just yet until the infrasturcture for matching sequence ID across multiple cameras is available.
83 */
85
86enum class ImuBiasStateIndex : int
87{
88 AccelBiasX,
89 AccelBiasY,
90 AccelBiasZ,
91
92 GyroBiasX,
93 GyroBiasY,
94 GyroBiasZ,
95
96 AccelScaleX,
97 AccelScaleY,
98 AccelScaleZ,
99
100 NumIndices,
101};
102
103typedef ceres::Jet<double, static_cast<int>(ImuBiasStateIndex::NumIndices)> ImuBiasJet;
104
105/*!
106 * A full-state IMU bias, with Accelerometer Bias, Gyroscope Bias, and Accelerometer Scale.
107 *
108 * Everything here belongs to one physical IMU, so a solve over several devices holds one of these per device. The
109 * strength of gravity is shared by all of them and lives in @ref WorldGravity instead.
110 */
111template <typename T> struct ImuBias
112{
113 static_assert(!std::is_const_v<T>,
114 "block scalar must not be const — did a `T*` overload deduce T = const double?");
115
116 static constexpr int kNumParameters = static_cast<int>(ImuBiasStateIndex::NumIndices);
117
118 static constexpr int kAccelBiasIndex = static_cast<int>(ImuBiasStateIndex::AccelBiasX);
119 static constexpr int kGyroBiasIndex = static_cast<int>(ImuBiasStateIndex::GyroBiasX);
120 static constexpr int kAccelScaleIndex = static_cast<int>(ImuBiasStateIndex::AccelScaleX);
121
122 Vector3<T> accel_bias;
123 Vector3<T> gyro_bias;
124
125 Vector3<T> accel_scale;
126
127 ImuBias()
128 : accel_bias(Vector3<T>::Zero()), gyro_bias(Vector3<T>::Zero()), accel_scale(Vector3<T>::Constant(T(1)))
129 {}
130
131 ImuBias(const Vector3<T> &accel_bias, const Vector3<T> &gyro_bias, const Vector3<T> &accel_scale)
132 : accel_bias(accel_bias), gyro_bias(gyro_bias), accel_scale(accel_scale)
133 {}
134
135 ImuBias(Eigen::Ref<const Vector<T, kNumParameters>> parameters)
136 : accel_bias(parameters.template segment<3>(kAccelBiasIndex)),
137 gyro_bias(parameters.template segment<3>(kGyroBiasIndex)),
138 accel_scale(parameters.template segment<3>(kAccelScaleIndex))
139 {}
140
141 void
142 pack(Eigen::Ref<Vector<T, kNumParameters>> parameters)
143 {
144 parameters.template segment<3>(kAccelBiasIndex) = this->accel_bias;
145 parameters.template segment<3>(kGyroBiasIndex) = this->gyro_bias;
146 parameters.template segment<3>(kAccelScaleIndex) = this->accel_scale;
147 }
148
150 seed() const
151 {
152 auto seeded = ImuBias<ImuBiasJet>();
153 seeded.accel_bias = {
154 {this->accel_bias.x(), kAccelBiasIndex + 0},
155 {this->accel_bias.y(), kAccelBiasIndex + 1},
156 {this->accel_bias.z(), kAccelBiasIndex + 2},
157 };
158 seeded.gyro_bias = {
159 {this->gyro_bias.x(), kGyroBiasIndex + 0},
160 {this->gyro_bias.y(), kGyroBiasIndex + 1},
161 {this->gyro_bias.z(), kGyroBiasIndex + 2},
162 };
163 seeded.accel_scale = {
164 {this->accel_scale.x(), kAccelScaleIndex + 0},
165 {this->accel_scale.y(), kAccelScaleIndex + 1},
166 {this->accel_scale.z(), kAccelScaleIndex + 2},
167 };
168 return seeded;
169 }
170};
171
172enum class PoseCovarianceIndex : int
173{
174 PositionX,
175 PositionY,
176 PositionZ,
177
178 RotX,
179 RotY,
180 RotZ,
181
182 VelocityX,
183 VelocityY,
184 VelocityZ,
185
186 NumIndices,
187};
188
189static constexpr int kPosePosCovIndex = static_cast<int>(PoseCovarianceIndex::PositionX);
190static constexpr int kPoseRotCovIndex = static_cast<int>(PoseCovarianceIndex::RotX);
191static constexpr int kPoseVelCovIndex = static_cast<int>(PoseCovarianceIndex::VelocityX);
192static constexpr int kNumPoseCovIndices = static_cast<int>(PoseCovarianceIndex::NumIndices);
193
194//! [position; rotation; velocity], the order computeImuResidual writes its residual in.
195typedef Matrix<double, kNumPoseCovIndices, kNumPoseCovIndices> PoseCovarianceMatrix;
196
197/*!
198 * Computes the radii and covariance orientation from the 3x3 position part of the pose covariance matrix
199 * (3σ elliphoir, so 97%-ish probability containment, assuming a perfectly matching noise model).
200 *
201 * @param covariance The raw pose covariance matrix to compute the radii and orientation from.
202 * @param[out] out_radii The radii of the position covariance will be written to this variable.
203 * @param[out] out_Q_origin_cov The orientation of the covariance will be written to this variable.
204 */
205void
206covariancePositionRadius(const PoseCovarianceMatrix &covariance, xrt_vec3 &out_radii, xrt_quat &out_Q_origin_cov);
207
208
210{
211 static constexpr int kNumResiduals = 9;
212
213 //! The preintegrated rotation from the IMU samples, as a quaternion.
214 Quaterniond Q_start_end;
215 //! This is really "delta integrated specific force", but we call it delta_velocity for simplicity.
217 //! This is really "delta double-integrated specific force", but we call it delta_position for simplicity.
219
220 //! Whether the body angular velocity is valid.
222 //! The body-frame angular velocity at the end of the window. Raw reading, apply calibration before using.
224
225 //! The whitening matrix for this preintegration, the lower triangular.
227
228 double dt;
229
230 Vector3d accel_bias_at_integration;
231 Vector3d gyro_bias_at_integration;
232
233 Vector3d accel_scale_at_integration;
234
235 /*
236 * Bias Jacobians
237 */
238
239 Matrix3d J_R_bg; //!< d(Q_start_cur) / d(gyro_bias)
240
241 Matrix3d J_v_ba; //!< d(delta_velocity) / d(accel_bias)
242 Matrix3d J_v_sa; //!< d(delta_velocity) / d(accel_scale)
243 Matrix3d J_v_bg; //!< d(delta_velocity) / d(gyro_bias)
244
245 Matrix3d J_p_ba; //!< d(delta_position) / d(accel_bias)
246 Matrix3d J_p_sa; //!< d(delta_position) / d(accel_scale)
247 Matrix3d J_p_bg; //!< d(delta_position) / d(gyro_bias)
248
250 Identity()
251 {
252 return {
253 .Q_start_end = Quaterniond::Identity(),
254 .delta_velocity = Vector3d::Zero(),
255 .delta_position = Vector3d::Zero(),
256 .body_angular_velocity_valid = false,
257 .body_angular_velocity = Vector3d::Zero(),
258
259 .whitening = PoseCovarianceMatrix::Zero(),
260
261 .dt = 0,
262
263 .accel_bias_at_integration = Vector3d::Zero(),
264 .gyro_bias_at_integration = Vector3d::Zero(),
265 .accel_scale_at_integration = Vector3d::Constant(1.0),
266
267 .J_R_bg = Matrix3d::Zero(),
268 .J_v_ba = Matrix3d::Zero(),
269 .J_v_sa = Matrix3d::Zero(),
270 .J_v_bg = Matrix3d::Zero(),
271 .J_p_ba = Matrix3d::Zero(),
272 .J_p_sa = Matrix3d::Zero(),
273 .J_p_bg = Matrix3d::Zero(),
274 };
275 }
276};
277
278/*!
279 * Does IMU pre-integration on a set of IMU samples, interpolates the starting sample to the start time, and returns the
280 * pre-integrated samples.
281 *
282 * @param imu_samples The IMU samples to preintegrate
283 * @param first_sample_after_end_time The first sample after the end timestamp
284 * @param imu_bias The Jet-seeded IMU biases
285 * @param start_time_ns The start time of the preintegration
286 * @param end_time_ns The end time of the preintegration
287 */
289preintegrate(const std::span<const xrt_imu_sample> &imu_samples,
290 const xrt_imu_sample &first_sample_after_end_time,
291 const ImuBias<ImuBiasJet> &imu_bias,
292 timepoint_ns start_time_ns,
293 timepoint_ns end_time_ns);
294
295
296/*
297 *
298 * IMU residuals
299 *
300 */
301
302template <typename T>
303static void
304computeImuResidual(const PreintegratedImuSamples &imu_preintegration,
305 const PoseWithVelocity<T> &start_T_world_device,
306 const PoseWithVelocity<T> &end_T_world_device,
307 const ImuBias<T> &imu_bias,
308 const WorldGravity<T> &world_gravity,
309 Vector<T, PreintegratedImuSamples::kNumResiduals> &residual)
310{
311 Quaternion<T> Q_world_device_start;
312 start_T_world_device.getQuaternion(Q_world_device_start);
313 auto start_Q_device_world = Q_world_device_start.conjugate();
314
315 Quaternion<T> end_Q_world_device;
316 end_T_world_device.getQuaternion(end_Q_world_device);
317
318 Vector3<T> gravity_vector = world_gravity.toVector();
319
320 // I'm sure we've all seen these exact formulas a million times.
321 auto end_predicted_Q_start = start_Q_device_world * end_Q_world_device;
322 auto predicted_start_delta_velocity = start_Q_device_world * //
323 (end_T_world_device.velocity - //
324 start_T_world_device.velocity - //
325 (gravity_vector * imu_preintegration.dt)); //
326 auto predicted_start_delta_position = //
327 start_Q_device_world * //
328 (end_T_world_device.translation - //
329 start_T_world_device.translation - //
330 start_T_world_device.velocity * imu_preintegration.dt - //
331 (T(0.5) * gravity_vector * imu_preintegration.dt * imu_preintegration.dt)); //
332
333 // Compute the deltas for how much IMU bias has changed from now to preintegration.
334 Vector3<T> delta_accel_bias = imu_bias.accel_bias - imu_preintegration.accel_bias_at_integration.cast<T>();
335 Vector3<T> delta_gyro_bias = imu_bias.gyro_bias - imu_preintegration.gyro_bias_at_integration.cast<T>();
336 Vector3<T> delta_accel_scale = imu_bias.accel_scale - imu_preintegration.accel_scale_at_integration.cast<T>();
337
338 // Correct the amount of rotation by the change in gyro bias
339 Quaternion<T> corrected_Q_start_end = //
340 imu_preintegration.Q_start_end.cast<T>() * //
341 quat_exp_so3(imu_preintegration.J_R_bg.cast<T>() * delta_gyro_bias); //
342
343 // Correct the change in velocity and position by the change by accel and gyro bias/scale.
344 Vector3<T> corrected_start_delta_velocity = //
345 imu_preintegration.delta_velocity.cast<T>() //
346 + imu_preintegration.J_v_ba.cast<T>() * delta_accel_bias //
347 + imu_preintegration.J_v_bg.cast<T>() * delta_gyro_bias //
348 + imu_preintegration.J_v_sa.cast<T>() * delta_accel_scale; //
349
350 Vector3<T> corrected_start_delta_position = //
351 imu_preintegration.delta_position.cast<T>() //
352 + imu_preintegration.J_p_ba.cast<T>() * delta_accel_bias //
353 + imu_preintegration.J_p_bg.cast<T>() * delta_gyro_bias //
354 + imu_preintegration.J_p_sa.cast<T>() * delta_accel_scale; //
355
356 // Compute the amount of rotation error for the predicted vs. measured
357 auto delta_rotation_error = corrected_Q_start_end.conjugate() * end_predicted_Q_start;
358 if (delta_rotation_error.w() < T(0)) {
359 delta_rotation_error.coeffs() = -delta_rotation_error.coeffs();
360 }
361
362 residual.template segment<3>(kPosePosCovIndex) =
363 predicted_start_delta_position - corrected_start_delta_position;
364 residual.template segment<3>(kPoseRotCovIndex) = quat_ln_so3(delta_rotation_error);
365 residual.template segment<3>(kPoseVelCovIndex) =
366 predicted_start_delta_velocity - corrected_start_delta_velocity;
367}
368
369/*
370 *
371 * Cost functors
372 *
373 */
374
375/*!
376 * IMU preintegration factor, between each camera keyframe.
377 *
378 * The keyframes are LED model states, which this sees the IMU at through the model -> IMU extrinsics.
379 */
381{
382 PreintegratedImuSamples preintegration;
383
384 template <typename T>
385 bool
386 operator()(const T *const world_gravity_parameters,
387 const T *const imu_bias_parameters,
388 const T *const imu_extrinsics_parameters,
389 const T *const start_keyframe_parameters,
390 const T *const end_keyframe_parameters,
391 T *residuals) const
392 {
393 const WorldGravity<T> world_gravity{
394 Map<const Vector<T, WorldGravity<T>::kNumParameters>>(world_gravity_parameters),
395 };
396
397 const ImuBias<T> imu_bias{Map<const Vector<T, ImuBias<T>::kNumParameters>>(imu_bias_parameters)};
398
399 const ImuExtrinsics<T> imu_extrinsics{
400 Map<const Vector<T, ImuExtrinsics<T>::kNumParameters>>(imu_extrinsics_parameters),
401 };
402
403 const PoseWithVelocity<T> start_T_world_model{
404 Map<const Vector<T, PoseWithVelocity<T>::kNumParameters>>(start_keyframe_parameters),
405 };
406 const PoseWithVelocity<T> end_T_world_model{
407 Map<const Vector<T, PoseWithVelocity<T>::kNumParameters>>(end_keyframe_parameters),
408 };
409
410 Vector<T, PreintegratedImuSamples::kNumResiduals> computed_residual;
411 computeImuResidual<T>(this->preintegration, //
412 imu_extrinsics.toWorldImu(start_T_world_model), //
413 imu_extrinsics.toWorldImu(end_T_world_model), //
414 imu_bias, //
415 world_gravity, //
416 computed_residual); //
417
418 Map<Vector<T, PreintegratedImuSamples::kNumResiduals>> out_residual(residuals);
419 out_residual = this->preintegration.whitening.cast<T>() * computed_residual;
420
421 return true;
422 }
423};
424
425typedef ceres::AutoDiffCostFunction<ImuCostFunctor, //
426 PreintegratedImuSamples::kNumResiduals, //
432 ImuCostFunction;
433
434//! IMU Bias random walk cost functor, between each keyframe.
436{
437 static constexpr int kNumResiduals = ImuBias<double>::kNumParameters;
438
439 //! Seconds spanned by this interval
440 double dt;
441
442 template <typename T>
443 bool
444 operator()(const T *const start_bias_parameters, //
445 const T *const end_bias_parameters, //
446 T *residuals) const //
447 {
448 const ImuBias<T> start{Map<const Vector<T, ImuBias<T>::kNumParameters>>(start_bias_parameters)};
449 const ImuBias<T> end{Map<const Vector<T, ImuBias<T>::kNumParameters>>(end_bias_parameters)};
450
451 assert(dt > 0);
452
453 const T sqrt_dt = T(std::sqrt(this->dt));
454
455 Map<Vector3<T>> accel_residual(residuals + 0);
456 Map<Vector3<T>> gyro_residual(residuals + 3);
457 Map<Vector3<T>> scale_residual(residuals + 6);
458
459 /*
460 * Assume that the difference between the start and ending bias is close to zero (it should be),
461 * whitened by how much we *expect* it to walk.
462 */
463 accel_residual = (end.accel_bias - start.accel_bias) / (T(kAccelBiasRandomWalkSigma) * sqrt_dt);
464 gyro_residual = (end.gyro_bias - start.gyro_bias) / (T(kGyroBiasRandomWalkSigma) * sqrt_dt);
465 scale_residual = (end.accel_scale - start.accel_scale) / (T(kAccelScaleRandomWalkSigma) * sqrt_dt);
466
467 // @todo Manually write the jacobians for these for performance
468
469 return true;
470 }
471};
472
473typedef ceres::AutoDiffCostFunction<ImuBiasRandomWalkCostFunctor, //
474 ImuBiasRandomWalkCostFunctor::kNumResiduals, //
477 ImuBiasRandomWalkCostFunction;
478
479//! IMU Bias anchor cost functor, to hold it at some value when marginalization has no guess for it.
481{
482 static constexpr int kNumResiduals = ImuBias<double>::kNumParameters;
483
484 ImuBias<double> anchor;
485
486 template <typename T>
487 bool
488 operator()(const T *const bias_parameters, //
489 T *residuals) const //
490 {
491 const ImuBias<T> bias{Map<const Vector<T, ImuBias<T>::kNumParameters>>(bias_parameters)};
492
493 Map<Vector3<T>> accel_residual(residuals + 0);
494 Map<Vector3<T>> gyro_residual(residuals + 3);
495 Map<Vector3<T>> scale_residual(residuals + 6);
496
497 // Assume the IMU bias is close to what we expect it to be.
498 accel_residual = (bias.accel_bias - this->anchor.accel_bias) / T(kAccelBiasAnchorSigma);
499 gyro_residual = (bias.gyro_bias - this->anchor.gyro_bias) / T(kGyroBiasAnchorSigma);
500 scale_residual = (bias.accel_scale - this->anchor.accel_scale) / T(kAccelScaleAnchorSigma);
501
502 // @todo Manually write the jacobians for these for performance
503
504 return true;
505 }
506};
507
508typedef ceres::AutoDiffCostFunction<ImuBiasAnchorCostFunctor, //
509 ImuBiasAnchorCostFunctor::kNumResiduals, //
511 ImuBiasAnchorCostFunction;
512
513}; // namespace xrt::tracking::constellation::optimizer
Definition utility_northstar.h:779
Definition utility_northstar.h:16
int64_t timepoint_ns
Integer timestamp type.
Definition u_time.h:77
int64_t time_duration_ns
Integer duration type in nanoseconds.
Definition u_time.h:88
constexpr double kMaxGyroBias
Largest gyroscope bias to allow, in rad/s. The BMI055's zero-rate offset is +-1 deg/s,...
Definition imu_preintegration.hpp:71
constexpr time_duration_ns kKeyframeGroupingToleranceNs
Camera samples from the same mosaic closer together than this are treated as one keyframe.
Definition imu_preintegration.hpp:84
PreintegratedImuSamples preintegrate(const std::span< const xrt_imu_sample > &imu_samples, const xrt_imu_sample &first_sample_after_end_time, const ImuBias< ImuBiasJet > &imu_bias, timepoint_ns start_time_ns, timepoint_ns end_time_ns)
Does IMU pre-integration on a set of IMU samples, interpolates the starting sample to the start time,...
Definition internal_math.cpp:406
Matrix< double, kNumPoseCovIndices, kNumPoseCovIndices > PoseCovarianceMatrix
[position; rotation; velocity], the order computeImuResidual writes its residual in.
Definition imu_preintegration.hpp:195
constexpr double kMaxAccelBias
Largest accelerometer bias to allow, in m/s^2. The BMI055's zero-g offset is +-70mg,...
Definition imu_preintegration.hpp:69
constexpr double kGyroNoiseDensity
BMI055 gyroscope noise density, 0.014 deg/s/sqrt(Hz) from the datasheet, in rad/s/sqrt(Hz).
Definition imu_preintegration.hpp:54
constexpr double kAccelNoiseDensity
BMI055 accelerometer noise density, 150 ug/sqrt(Hz) from the datasheet, in (m/s^2)/sqrt(Hz).
Definition imu_preintegration.hpp:56
void covariancePositionRadius(const PoseCovarianceMatrix &covariance, xrt_vec3 &out_radii, xrt_quat &out_Q_origin_cov)
Computes the radii and covariance orientation from the 3x3 position part of the pose covariance matri...
Definition internal_math.cpp:295
Generic math helpers for constellation optimizer code.
Interoperability helpers connecting internal math types and Eigen.
Base implementations for math library.
Helpers for quatexpmap math for bigceres usage.
C++-only functionality in the Math helper library.
Definition m_documentation.hpp:15
Eigen::Quaternion< typename Derived::Scalar > quat_exp_so3(Eigen::MatrixBase< Derived > const &vec)
Fully-templated free function for quaternion exponentiation, SO(3) version as described by Grassia.
Definition m_quatexpmap.hpp:119
Eigen::Matrix< Scalar, 3, 1 > quat_ln_so3(Eigen::Quaternion< Scalar > const &quat)
Fully-templated free function for quaternion log map, SO(3) version.
Definition m_quatexpmap.hpp:148
Wrapper around OS native time functions.
IMU Bias anchor cost functor, to hold it at some value when marginalization has no guess for it.
Definition imu_preintegration.hpp:481
A full-state IMU bias, with Accelerometer Bias, Gyroscope Bias, and Accelerometer Scale.
Definition imu_preintegration.hpp:112
IMU Bias random walk cost functor, between each keyframe.
Definition imu_preintegration.hpp:436
double dt
Seconds spanned by this interval.
Definition imu_preintegration.hpp:440
IMU preintegration factor, between each camera keyframe.
Definition imu_preintegration.hpp:381
The LED model -> IMU rotation.
Definition internal_math.hpp:377
Pose with 9 degrees of freedom, translation, rotation, and linear velocity.
Definition internal_math.hpp:340
Matrix3d J_p_sa
d(delta_position) / d(accel_scale)
Definition imu_preintegration.hpp:246
Vector3d body_angular_velocity
The body-frame angular velocity at the end of the window. Raw reading, apply calibration before using...
Definition imu_preintegration.hpp:223
Matrix3d J_v_sa
d(delta_velocity) / d(accel_scale)
Definition imu_preintegration.hpp:242
PoseCovarianceMatrix whitening
The whitening matrix for this preintegration, the lower triangular.
Definition imu_preintegration.hpp:226
Quaterniond Q_start_end
The preintegrated rotation from the IMU samples, as a quaternion.
Definition imu_preintegration.hpp:214
Matrix3d J_v_bg
d(delta_velocity) / d(gyro_bias)
Definition imu_preintegration.hpp:243
Matrix3d J_p_bg
d(delta_position) / d(gyro_bias)
Definition imu_preintegration.hpp:247
bool body_angular_velocity_valid
Whether the body angular velocity is valid.
Definition imu_preintegration.hpp:221
Matrix3d J_p_ba
d(delta_position) / d(accel_bias)
Definition imu_preintegration.hpp:245
Matrix3d J_v_ba
d(delta_velocity) / d(accel_bias)
Definition imu_preintegration.hpp:241
Vector3d delta_position
This is really "delta double-integrated specific force", but we call it delta_position for simplicity...
Definition imu_preintegration.hpp:218
Matrix3d J_R_bg
d(Q_start_cur) / d(gyro_bias)
Definition imu_preintegration.hpp:239
Vector3d delta_velocity
This is really "delta integrated specific force", but we call it delta_velocity for simplicity.
Definition imu_preintegration.hpp:216
The strength of gravity in the world, as a parameter block of its own.
Definition internal_math.hpp:193
Vector3< T > toVector() const
Gravity as a vector, which points along +Y since world up is -Y in the OpenCV space the optimizer wor...
Definition internal_math.hpp:219
IMU Sample.
Definition xrt_tracking.h:143
A quaternion with single floats.
Definition xrt_defines.h:246
A 3 element vector with single floats.
Definition xrt_defines.h:310
#define U_TIME_1MS_IN_NS
The number of nanoseconds in a millisecond.
Definition u_time.h:54